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The work described in this report was performed by the Guidance and Control 
Division of the Jet Propulsion Laboratory. 
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Errata for Technical Report 32-1565 


Since the users of this report will certainly be referring to JPL TR 32-1565 
(Ref. 9 ) for a much more detailed treatment of hinge-connected rigid body sys- 
tem equations and their derivation, it seems appropriate here to add a few tech- 
nical remarks that may contribute to that reports utility and to correct several 
significant errors in the text which have been brought to the authors attention. 

(1) In the last paragraph on page 30, the hinge torques x 1 , t 2 , and t 3 are referred 
to. The symbol x> means r>g ; . 

_(2) On page 35, when considering the significance of Eq. (34), note that C T C = 
(C + C) T (C + C) = (17 — &)(U + C) ^ V + ■ • * (2nd-degree terms), so orthog- 
onality is preserved in the linear approximation. Still, for planar motion (all gs the 
same), it would be impossible to associate an angle of rotation with C, since the 
cosine = 1 and the angle is large. Thus orthogonality is preserved, but physical 
interpretation is jeopardized, 

(3) Note the text corrections that follow. 
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Abstract 


This report describes several computer subroutines designed to provide the solu- 
tion to minimum-dimension sets of discrete-coordinate equations of motion for 
systems consisting of an arbitrary number of hinge-connected rigid bodies assem- 
bled in a tree topology. In particular, these routines may be applied to (1) the case 
of completely unrestricted hinge rotations, (2) the totally linearized case (i.e., all 
system rotations are small), and (3) the mixed, or partially linearized, case. The use 
of the programs in each case is demonstrated using a five-body spacecraft and atti- 
tude control system configuration. The ability of the subroutines to accommodate 
prescribed motions of system bodies is also demonstrated. Complete listings and 
user instructions are included for these routines (written in Fortran V) which are 
intended as multi- and general-purpose tools in the simulation of spacecraft and 
other complex electromechanical systems. 
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Attitude Dynamics Simulation Subroutines 
for Systems of Hinge-Connected 
Rigid Bodies 


I. Introduction 

Since Hooker and Margulies (Ref. 1) published their derivation of the vector- 
dyadic equations of motion for an arbitrary number of point-connected rigid bodies 
in a topological tree, a number of computer programs (Refs. 2-7) based on these 
equations and similar generic formulations have come into wide use in the aero- 
space industry. Meanwhile, Hooker (Ref. 8) has devised a procedure for reducing 
the original Hooker-Margulies equations to a minimum-dimension set (i.a, equal 
to the number of degrees of rotational freedom) by summing selected subsets of the 
equations (paralleling Russell’s approach— Ref. 2) and dot-multiplying by unit vec- 
tors associated with the connecting joint geometry. 

In Reference (9) a variation of Hookers procedure was used to derive a 
minimum-dimension set of equations of motion for a hinge-connected system of 
rigid bodies. (It is always possible to substitute a hinge-connected system for a 
point-connected system by introducing auxiliary bodies and without changing the 
dimension of the problem.) The objective was, having derived this set of discrete- 
coordinate equations in a highly systematic and detailed way, to (1) isolate certain 
subsets of the system’s rigid bodies that may be considered collectively as a flexible 
substructure, (2) linearize in the variables describing relative motions of contigu- 
ous bodies within the substructure, and (3) transform from substructure discrete 
variables to distributed (modal) coordinates which may then be truncated for com- 
putational efficiency, The result is, in general, a new kind of hybrid-coordinate 
(discrete + distributed) formulation which can accommodate large gross deforma- 
tions of an appendage even though its local deformations are small. 
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However, while computer subroutines which solve the hybrid-coordinate equa- 
tions of Ref. 9 will be discussed in a subsequent report, this report will confine itself 
to descriptions of subroutines which solve only the discrete-coordinate equations 
of motion for the hinge-connected system, Specifically, it will describe and apply 
these programs as dynamic simulation tools for three possible situations: (1) the 
case in which all the system bodies are permitted large or unrestricted rotations 
(Section II), (2) the case in which all system rotations (and their derivatives) are 
small, allowing total linearization of the equations (Section III), and (3) the mixed 
case of large and small rotations for which partial linearization is appropriate 
(Section IV). 

The programs, since they are based on the discrete-coordinate equations of 
Ref. 9, will differ from those developed in Ref. 4 in several significant respects. 
First, they will be of minimum dimension. Also, interbody coordinate transforma- 
tions are accomplished internal to the programs, the user being required only to 
arbitrarily specify some nominal system configuration. Input has been simplified. 
Pains have been taken to minimize internal coordinate transformations and other 
computations to improve speed. And, finally, the user may, if he desires, prescribe 
the motion of some system bodies rather than allow the subroutine to calculate it. 

The subroutine approach to solving attitude dynamics equations is a logical con- 
sequence of the approach taken historically at JPL to develop computer programs 
(analog or digital) specialized to each particular control mode or portion of a con- 
trol mode under study, e.g., the thrust vector control mode during propulsive tra- 
jectory corrections. In contrast, however, to the narrow specialization of each 
simulation program to specific control problems has been the extensive use of 
general-purpose simulation languages to facilitate the creation of such programs. 
Over the past decade, such digital simulation languages as MIDAS, MIMIC, 
DSL/90, and most recently, CSSL III (Continuous System Simulation Language) 
have proved extremely valuable since they relieve the programmer of the worry 
over numerical integration routines, and since they can properly sort the many, at 
times highly interrelated, computational statements which occur and thereby help 
to ensure a correct solution. 

Many of these simulators also provide a powerful array of useful special func- 
tions, in addition to the standard Fortran library of mathematical and intrinsic 
functions, such as hysteresis loops, noise generators, time delays, pulse generators, 
logic gates, etc. In fact, provision is also made for inclusion into the language of 
any user-created functions or subroutines. An example of an extremely useful sub- 
routine that has been made a part of the CSSL III processor is HCK (Hamilton- 
Cayley-Klein), a rigid-body attitude dynamics package, written by E. II. Kopf. 
HCK calculates the angular acceleration components of a single rigid body, given 
its angular velocities, applied torques, and inertial properties. The HCK param- 
eters (i.e., Euler parameters) are used in the necessary kinematic calculations and 
provide body-to-inertial (or vice versa) coordinate transformations as desired. Euler 
angles may also be requested to indicate the angular position of the body with 
respect to the inertial reference frame. 

Just like HCK, the subroutines to be described here calculate angular accelera- 
tions, except that they do so for every body in a system of hinge-connected rigid 
bodies. All integration operations are confined to the main CSSL III simulation pro- 
gram (which calls its centralized numerical integration routines as required). Thus, 
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performing no numerical integration and required only to compute angular accel- 
eration components (given the necessary system states, forcing functions, and 
physical properties), the routines are intended simply as very specialized and yet 
powerful simulation tools for application to a wide variety of current space vehicle 
attitude control problems. 


II. Unrestricted Systems 

A. System Mathematical Model 

Figure 1 illustrates a set of eleven hinge-connected rigid bodies, labeled accord- 
ing to a convention designed to facilitate the processing of the equations of motion. 
This example will be useful in illustrating and interpreting the general mathemati- 
cal model and the labeling conventions adopted here for its description. (In reading 
the list of conventions and symbol definitions, frequent reference should be made 
to Fig. 1.) Only after entertaining the complete cast of symbols used in what follows 
will consideration be given to the much smaller list of symbols to which values must 
be assigned by the user of the computer programs. 


The mathematical model consists of a set of n -h 1 rigid bodies interconnected 
by n hinges 1 ; the indicated numbers of bodies and hinges imply a tree topology. 
Any interbody connection forces in addition to those at the n hinges must be 
treated as forces external to the entire system. (If there were a linear spring con- 
necting the mid-point of body in Fig. 1 to some point on body A 0} for example, 
it would be necessary to replace the spring with a pair of equal and opposite forces 
applied to the spring attachment points on An and A a ; the magnitude and direction 
of these forces would have to be expressed in terms of the unknown kincmatical 
variables of the system.) 

I. Definitions of fundamental symbols 
Def. 1. Let n be the number of hinges interconnecting a set of n + 1 bodies. 

Def. 2. Define the integer sets £8 = (0, 1, • • • , n} and & = {1, * * * ,n}. 

Def . 3. Let A 0 be a label assigned to one body chosen arbitrarily as a reference 
body, and let A u ■ ■ • ,A n be labels assigned to the remaining bodies in 
such a way that if Aj is located between A 0 and A ky then 0 <j<k, 

Def. 4. Define the dextral orthogonal sets of unit vectors b^b^b* so as to be 
imbedded in A\ for k e£8, and such that in some arbitrarily selected nomi- 
nal configuration of the system b* = b^for a = 1, 2, 3 and fc, / e£8. 

Def. 5. Define {b fc } as the column array of unit vectors 2 


1 The word “hinge” as used here implies a connection which maintains a line common to both 
bodies; such a connection is sometimes called a line hinge . 

2 Note that braces { } are used in this report both to identify column arrays of vectors and to 
enclose sets of integers; the distinction is apparent as soon as the objects within the braces are 
identified as vectors (boldface) or scalars. 
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Def. 6. Let co° = {b°} r 6) D be the inertial angular velocity vector of / 0 , so that <o° 
is the corresponding 3-by-l matrix in vector basis {b 0 }. 

Def. 7. Let c * be the mass center of for ke£B (see Fig. 1). 

Def . 8. Let be a point on the hinge axis common to 4 k and 4. } for / < k and 
(see Fig. 1), 

Def. 9. Define the kth neighbor set for k e£B such that if 4j is attached 
to 


4 

Def . 10. For j e£S fc> let p* ; = {VjV* be the position vector with respect to c k of 
the labeled point on the hinge axis (either fc or /*■*) which is common to 
4 k and so that p* j is the corresponding 3-by-l matrix in vector basis 
{b*}. (See Fig. 1, and note the special cases p 21 = p 2S — p 32 = p S4 — p 43 = 0.) 



Fig. 1. Eleven rigid bodies interconnected by ten hinges 
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Def. 11. Let g* == {b*} T g* be a unit vector parallel to the hinge axis through so 
that g k is the corresponding 3-by-l matrix in vector basis {b fc }. 

Def. 12. For k€0 ) J let y k be the angle of a g ?f -rotation of with respect to the body 
attached at fa, that is, a rotation during which a right-handed screw fixed 
in with its axis parallel, to g fc advances in the direction of g fc . Assign the 
value zero to y * for k € Q> when the system is in its arbitrarily chosen nomi- 
nal configuration, for which b* = b a = 1, 2, 3; fc, / € 85. 

Def. 13. Let m k be the mass of 4 k for k e £3. 

Def. 14. Let l k = (b fe } :r J fc {b & } be the inertia dyadic of for c h , so that l k is the 
corresponding inertia matrix in vector basis {b*}, i.e.. 



r p 

P 

*12 

P 1 

l k = 

p 
1 12 

*22 

P 

*23 


P 

L *13 

P 

*24 

P 

£ 33 J 


Def. 15. Let F* = be the resultant vector of all forces applied to 4 k except 

for those due to interbody forces transmitted at hinge connections , so 
that F k is the corresponding 3-by-l matrix in vector basis {b fc }. (Thus, any 
interbody forces due to spring connections, etc., which are separate from 
the n hinge connections would contribute to F k , as would all forces from 
sources external to the system.) 

Def. 16. Let T Ji == {b fc } r T* be the resultant moment vector with respect to c k of 
all forces applied to except for those due to interbody forces transmitted 
at hinge connections , so that T k is the corresponding 3-by-l matrix in vec- 
tor basis {b fc }. 

Def. 17. Let be the scalar magnitude of the torque component applied to in 
the direction of g fc by the body connected at /t k . 

Note that Def. 3 allows a certain freedom in labeling the system bodies after the 
reference body has been chosen, the requirement always being that label values 
increase as one progresses outward from 4 0 along any chain of bodies. Figure 2 
shows, for example, two equally acceptable ways of labeling a seven-body system. 

Def. 18. Let the tilde symbol (^) signify in application to a 3-by-l matrix V with 
elements V© (S = 1, 2, 3) transformation to a skew-symmetric 3-by-3 
matrix V. Thus, the matrix V has the expanded form 

" o -v 3 vr 

Va 0 -V, 

-v 2 V, 0 _ 
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Fig. 2. Alternate labeling schemes for a system of seven bodies 


2. Definitions of derived symbols 
Def ; 19. Define the scalar such that for k and s e 

a ( lifA lies between / 0 and 

£«?£ ^ 

( 0 otherwise 

The term path elements will be applied to the scalars which art 
n (n 4- 1) in number. 

Def. 20. Let C rj be the direction cosine matrix defined by 

{b'J = C'i {bO 

for r, feff}. In the nominal state, y* = 0 for &€<£?, and all unit vectors of 
corresponding subscript are aligned, so that C ri then is 17, the 3-by-3 unit 
matrix, 

Def . 21 . Define Q'W « 2 ^ so that is the total system mass. 
fc€$ 
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Def. 22. Define Qyikj = the total mass on the branch of the system attached to 
and commencing with Aj. 

Def . 23. For / € {£Q — define = the vector from c h to the hinge point attached 
to Ak and on the path leading from A k toward A r (In Fig, 1, for example, 

L 5G — L 57 = L 58 — p 5G and L 3i ~ 0 for /e£B.) (Define — 0,) 

Def. 24. Define {b' ; } r D a 4 D» = - 2 for fc«£B. 

/«£B 

Def . 25. Let b* be a point called the barycenter of A k and fixed relative to such 
that is the position vector of c k with respect to b*. 

Def, 20. Define (b fc } a ’D w ^ * D** -f for fc, Thus, is the vector 

from b k to the hinge point fixed in A* and on the path leading to Aj. 

Def. 27. Define the dyadic 

<t>'* 4 |* + m h (D w • D a U - D^D**) 4- 2 0 Tlkj (D« • D^U - 
where U is the unit dyadic. 

Def . 28. For ; e {(B — k} y define the dyadic 

O *' t — QtJ (D ,7c *D lt, U - D'*D*0 

3. Augmented bodies and bary centers . The barycenter bk of body Ak is defined 
in Def, 25 in mathematical terms which admit an interesting interpretation as a 
physical property of an abstraction called the augmented body. If one imagines 
the body Ah to be augmented by the addition of a particle of mass at the con- 
nection point of A k with Aj (for /efij*), then the result is called the fcth augmented 
body, here referred to as A*. Since tyyi kj is the total mass of the branch of bodies 
attached to Ak commencing with Aj , the mass of each augmented body is O'??, the 
total system mass. 

Definition 24 provides 

— QMD** — 2 

jeftt 

which reveals the fact that — D kk is the position vector of the mass center of A* with 
respect to the mass center c k of b }; . Thus, the barycenter b k is the mass center of the 
fcth augmented body Ah- With this interpretation comes the relationship 

2 m(D hj = m/J + 2 — 0 

;e<(B k 

Since D kk is the position vector from the barycenter b* to the mass center c k of Ak, 
and (from Def. 26) is the position vector from b k to a particle of mass OTlkj 
located at the connection point of A k leading to body A jy for / e (Bk, one can interpret 
$> kk from Def. 27 as the inertia dyadic of the augmented body A I with respect to the 
barycenter bn. Such physical interpretation is not possible for the dyadic defined 
in Def. 28, since this dyadic involves vector fixed in Aj and vector fixed in Ah- 
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Fig. 3. Augmented/' from Fig. 1 


In illustration of the concepts just described, Fig. 3 shows the augmented body 
/£ associated with body / 0 in the system of Fig. 1. 

B. The Dynamical Equations 

Using the symbols defined in the preceding section, the minimuimdimension set 
of equations for the rotational motion of the hinge-connected system (i.e., with n 
line hinges) can be written in vector-dyadic form as 

2 [T* + 2 D*' X F' - Q kk * X ***• ca* + Oft 2 V kj X D>*] = 0 

j€& ic£3-k 

(la) 


tv + g r " 2 *** [T* + 2 X F f — * a> fc — to* X ® kk * <o fc 

fce# 1 jc& 

+ Qn V D*'XD'*]=0 (lb) 

This result is, of course, the same as that derived by Hooker (Ref. 8) and proved in 
Ref. 9, Equation (lb) provides n scalar equations, and the vector equation (la) com- 
pletes the minimum-dimension set of n -f 3 scalar equations. By substituting the 
following kinematic expansions. 


and 


T> ik = i** X T> jk + X (to* X D^) 


= tt* 4- 2 (g*y. + <■>* x g*y s ) 

the vector-dyadic equations can be written in the form; 

a oo <( > + 2 a ft *y a = 2 a* 

ke£B 

and (for r e&) 

a rc . W° + 2 Or.y. = g r ‘ 2 + T r 


( 2 ) 

(3) 

(4a) 

(4b) 
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where 


Def. 29. a 00 = 2 [•** ~ On 2 (D^-D^U - D'W)] = 2 2 ®* 7 

kim H&-k k€® /e£B 

Def. 30. a w = 2 [*,*«** - QW 2 «.* (D jfc -D* 7 U - D'W)] *g* 

= 22 e.^'g* 
k€£8 

Def. 31. a r ° = g r • 2 e rk [<D S * — On 2 (£> 7 * • D*’U - D 7ft D* 7 )] 

fe&-k 

= g r • 2 2 £rfcf fc7 ' 

*€<9 

Def. 32. a rs = g r • 2 *nt 1®“ - QW 2 (D* * t>"U - D J *D* 7 )] • *^g s 

= g r * 2 2 erhe»jQ kl ‘ g a 

?C0 

Def. 33. A k = T* + 2 D fci X F 7 - w fc X 4> fcjc * to 71 

feS 

-hOM 2 P* 7 ' X [ W> X {0> 7 X D 7 *)] } 

le£B-k 

- 2 * 2 e.*y. («* X g s ) 

/«£8 


A matrix equivalent to Eqs, (4a) and (4b) may be obtained by defining the 
following symbols: 

Def . 34 . Define the 3-by-3 matrix in vector basis {b D ) as a 00 = {b 0 } * a 00 * {b 0 } T . 

A 

Def ; 35. For se& y define the 3-by-l matrices in basis {b 0 } as a os = {b*} - a 08 , and 
let a s o = aj g . 

De/. 36, For k e£53, define the 3-by-l matrices in basis {b 0 } as Ek = {b 0 } * A*, 


In terms of these symbols, the system matrix equation can be written as 


1 

a 

o 

o 

#01 

#02 

. , . 

#0n 




i 

w| 

1 

#10 

#11 

#12 

■ * • 

#m 


Vi 


g lT c i » 2 ««*e» + Ti 

ktff 

#20 

#21 

#22 

• 

#2» 


y 2 

= 

g‘ T C 20 2 «2* E* + t 2 

1 

& 

3 

o 

#Wl 

#«2 

* ‘ * 

#nn 


% 


g nT C n# 2 e «S £|t + T n 

kes> J 


The coefficient matrix on the left side of (5) is a real, nonsingular, symmetric 
matrix which varies with time as the kinematical variables yi, * * 1 , y« change in 
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value. In general, then, the solution to this set of differential equations requires 
(e.g., when using numerical integration) repeated solutions of the matrix equation, 
(he., at every integration step). 

If one wishes to replace any variable yj (/ € &) by a known function of time, then 
one can remove VV from the angular acceleration matrix on the left side of Eq. (5), 
removing also the corresponding row and column of its coefficient matrix, and then 
rewrite on the right side those terms just deleted by the removal of the column on 
the left. This will be performed automatically by the subroutines if the user desires. 

Equation (5) constitutes a complete set of rotational dynamics equations , but 
they are not fully descriptive of the system motion until they are augmented by 
control equations specifying r a , • ■ ■ , T n and certain external forces and torques, 
and augmented also by kinematical equations as provided in the next section. 

C. Kinematical Equations 

The kinematical variables adopted in the preceding sections are as follows: yi> for 

k (Def. 12); C rj for r, j €<$ (Def, 20); and = {b 0 } • co° (Def. 6). Although the 
equations of motion have been expressed in terms of these quantities, the latter are 
not all independent. Relationships among kinematical variables developed in this 
section must therefore either be considered in conjunction with the dynamical 
equations or be substituted into them to remove redundant variables whenever a 
solution is sought. 

The direction cosine matrix C ri (Def. 20) relates sets of orthogonal unit vectors 
fixed in S T and A. h and hence depends upon those angles y# for which lies 
between 4 f and and also upon the corresponding unit vectors g" defining the 
intervening hinge axes. For the special case in which 4 T and are contiguous and 
/ < r, it is always possible (see Ref. 9) to express C ri (and G }r ) in terms of the single 
angle y r and the single matrix g r , as follows: 


C r * — U cos y r — g r sin y r + g r g r9 ‘ (1 — cos y r ) (6) 

and 

C* r = U cos y r + sin y r 4- g r g rT (l ~ cos y r ) “ ( C ri ) T (7) 

Definitions 34, 35, and 36 make clear that all n + 3 scalar equations in (5) are 
described in the vector basis {b 0 } of the reference body. This is for the purpose of 
minimizing the number of coordinate transformation computations that must be 
performed by the subroutines. Thus, it becomes unnecessary to calculate C rj for 
all r €f8, /€£8, It is only required that C rj be determined where and are con- 
tiguous and, since C 0r — C°?Ci r , to then derive matrices C 0r for An algorithm 
for accomplishing this task is described in Appendix A. 


D. System Specification 

Once the appropriate equations from the three preceding sections are embodied 
in a computer program, the user need not deal directly with all of the symbols and 
concepts introduced in the formulation of these equations. He can provide the 
computer with the very limited body of input required to specify his system, and 
then concentrate on interpreting the numerical integration output. 
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It is assumed that the engineer in his wisdom and experience has devised for 
his physical system a mathematical model consisting of a system of n 4- 1 bodies 
interconnected by n hinges (such as that illustrated in Fig. I). He has labeled the 
bodies as indicated by Def. 3 (Section II A), and fixed in his mind a nominal con- 
figuration (for which = 0 for ke£P and b* — b^for /, ke£B and a = 1,2,3). One 
final aid, however, will be needed to communicate the system’s interconnection 
pattern to the computer subroutine. 

Def . 37. For let h k denote the index of the body attached to 4 k and on the 

path leading to 4 f) , The scalars h J( will be termed “connection elements/’ 
(In Fig. 1, h, = 0, h 2 = 1, h z = 2, h 4 = 3, fc 5 = 0, h« = 5, h 7 = 6, h 8 = 7, 
h 9 = 8, and h 10 = 6.) 

With the definition of h ky it is now possible to specify a complete set of input 
information for the subroutine: 

Subroutine input (required) 

(1) The integer n (see Def. 1). 

(2) The n connection elements h k (k € &) (sec Def. 37). 

(3) For fte£S and the 3-by-l matrices p ki (see Def. 10). 

(4) The n 3-by-l matrices g* for ft e <£? (see Def. 11). 

(5) The n + 1 masses m k for ft e £3 (see Def. 13). 

(6) The n + 1 3-by-3 inertia matrices P for ft € £6 (see Def. 14). 

(7) For fte£3, the 2 (n -f 1) 3-by-l matrices F k and T k (see Defs. 15 and 16). 

(8) For ft e £?> the n scalar values r/ c (see Def, 17), 

(9) Values for the 2n scalars y * and y* for ft e<£? (see Def. 12) and the three ele- 
ments of the angular velocity matrix oj° (see Def. 6). 

Subroutine input (optional) 

Any kinematically prescribed variables, such as y ; (t) for some / e£P. As noted in 
Section IIB, it is possible to modify Eq. (5) so as to accommodate the substitution 
of an explicit function of time for a variable, such as y ? - (t), which in Eq, (5) is treated 
as an unknown. If this option is elected, the computer program input must include 
the specification of variables to be prescribed and appropriate values for y /, yy, 
and y j. 

No additional input is required. The subroutine is programmed to construct 
numerically all intermediate concepts (such as the path elements e 8 k for ft € s e 
the barycentric position matrices D kk and D kj for ft, /€$, the augmented body 
inertia matrices $ kk y etc. It can then evaluate numerically the functions displayed 
in Eq. (5) and provide the solution for and y 2 , ■ • ■ , y n . The subroutine and 
explicit details of its use are described in the following section. 

E. Subroutine MBDY Usage 

The computer subroutine, called MBDY, which provides the solution to Eq. (5), 
i.e., the solution to the unrestricted rotations of an arbitrary, hinge-connected, 
rigid-body tree system, may be exercised by means of either of two call statements. 
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The first of these initializes the program, supplying it with data which will remain 
constant throughout the dynamic simulation run, 

Initializing Call Statement 

CALL MBDY(NH,H,MB,MA,PB,PAAPI) 


where 


NH = the integer n — number of system hinges. 

H (k) = array containing the connection elements k e 
(H(l )=h l ,H(2)=f h , ■ * • , H(n) = h n ) 

MB(/) = array of reference body (/ 0 ) inertial constants; j — 1, ■ * - ,7 
(Specifically: MB(1) - /?,> MB(2) = /«,. MB(3) - ZJ S , 

MB(4) = -/? 2 , MB(5) - -7J 3 , MB(6) = -IJ S , and MB (7) - -m 0 .) 

MA(ij) = array of appendage body (6 U ■ * ■ ,^ n ) inertial constants; ie£P, 

j = 1, ■ ' 1 ,7(Thus, MA(i,l) = /* 11 ,MA(t,2) = • * - , MA(f,7) - m*.) 

PB(/,/) = array containing elements of p oi ; / ~ 1,2,3 

PA = array containing elements of p i j ; ie Q ) > j € £Zh, k ~ 1, 2, 3 

(Exception!! If j < t, set PA (i>ijc) = p l K Example: PA(3,3,1) — pf . 
All PA(i,/,fc), where / < i, will be ignored,) 

G(y) — array containing elements of j — 1,2,3 

PI« = array of prescribed variable indicators; i€c^?(If y i is a prescribed 
variable, PI ( i ) = 1. Otherwise, PI (i) — 0.) 

The statement CALL MBDY( NH,H, . . .) need only be executed once prior 
to a simulation run, but subsequently the subroutine must be entered (at every 
integration step) to compute the angular accelerations w°, y ly ■ • * ,y„, i.e., the sys- 
tem's dynamic response to its forcing functions. This is accomplished by executing 
the "dynamic" call statement. 

Dynamic Call Statement 

CALL MRATE(NH,TH,TB,TA,FB,FA,GM,GMD,GMDD,WO,WDOT) 
where 


NH = the integer n — number of system hinges 
TH(i) = array containing the hinge torques, t*; i e£P 
TB(/) — array containing the elements of T°; ; = 1, 2, 3 
TA(i,/) — array containing the elements of T'; i e £?, j = 1, 2, 3 
FB(/) — array containing the elements of F°; j = 1, 2 , 3 
FA(i,/) = array containing the elements of F'; i e<£P, j = 1, 2, 3 
GM(i) = array of the angles, yi\ie& 

GMD(i) = array of the angular velocities, y* ; i e & 

GMDD(i) = array of the prescribed angular accelerations, y*; ic^P 
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WO(/) = array containing the components of <o°; / = 1, 2, 3 

WDOT(/) = solution vector containing the elements of and y x , * ’ ‘ * 

/ = 1, ■ * , n + 3 (WDOT(l) - A*. WDOT(2) = A®, WDOT(3) - 

A®, WDOT(4) - y 1} WDOT(5) = y 2 , • ■ • , WDOT(n + 3) = y n ) 

The solution vector array, WDOT, is returned to the main calling program by 
the subroutine and contains the current computed value for the reference body’s 
angular acceleration relative to the inertial frame and the relative angular accel- 
erations y i of the remaining appendage bodies. 


To summarize, the call to MRATE supplies the subroutine with current instan- 
taneous values for hinge torques and externally applied torques and forces on the 
system. Explicit expressions for computing the T\ F\ and 7i, often as functions of 
the y i and yi and other system or control variables, are located in the main simula- 
tion program (see sample problem which follows). Current values of <*>°, yi, and y* 
are continuously produced by the main program’s numerical integration operators 
(operating on the solution vector components, WDOT(/)) and are therefore always 
available for input to the subroutine. The y* inputs to the subroutine are used only 
when the motion of certain system bodies is prescribed. 


When the MBDY subroutine is used, the main calling program must contain 
Fortran V (or IV) statements which specify “type” and allocate storage for the 
variables and arrays being used and assign values to these variables and arrays. 
Some examples of this will be shown in the next section, However, the mandatory 
specification statements are listed here. 


Required Specification Statements 

INTEGER NH, H(n), PI(n) 

REAL MB(7), MA(n,7), PB(n,3), PA(n,n,3), G(n,3), TH(n), 
TB(3), TA(n,3), FB(3), FA(n,3), GM(n), GMD(n), 
GMDD(n), WO(3) 

DOUBLE PRECISION WDOT(n 4- 3) 


Also, in order that storage allocation for arrays internal to the subroutine MBDY 
be minimized (i.e., made consistent with n hinges and no more), the following 
Fortran V (or IV) statement must appear in the subroutine ; 

PARAMETER Q = n 

The proper placement of this statement in MBDY is shown in the subroutine listing 
(Appendix A). 

F. A Five-Body System Simulation 

To illustrate the application of the MBDY subroutine for the unrestricted motion 
case, the space vehicle configuration shown in Fig. 4 will be simulated, The vehicle 
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is idealized to consist of a rigid central body, which will be designated as the refer- 
ence body /o, including the bus, solar panels, gas jets, sensors, communications 
equipment, etc. 

Hinged to the reference body is a rotating hub, labeled 4 U which in turn is 
hinged to platform 4 2 > carrying a television camera. The platform hinge axes 
located by g 1 and g 2 are orthogonal, g 1 = b° (-f Z-direction), and the rotation 
angles yi and y 2 are arbitrarily defined to be zero when the TV boresight unit vector 
u is in the Y-direction. Also, u is defined perpendicular to g 2 . Thus, we may write 

u — - sin y x cos y^bl 4- cos y 1 cos y 2 b* — sin y 2 b° 
g 8 = -hi 

A large instrumented boom, / 4 , is also hinged to the reference body by the 
mutually orthogonal and coplanar axes defined by g 3 and g 4 . (In this case, the inter- 
mediate rigid body, 4^ will be assigned zero mass.) Therefore, 

g s = bj 

The following list of system parameters will be used to define the system and 
initialize the MBDY subroutine: 

(1) n — 4 

(2) K - 0 , fc, = 1, h 3 = 0, h 4 = 3 

(3) p<" — [0 0 — 1.5] r , p™ = [0 -1.20 0] T meters 

p 1 0 ^[0 0 — 0.75] 3 ', p 12 - [0 -0.10 — 0J5] r meters 

p 21 = [0 “0.22 0.20 ] 71 meters 

p 30 = [0 0 0] r , p 34 = [0 0 0] T meters 

p 43 =f.[ 0 3.3 0] r meters 

(4) g 1 — [0 0 1] T meters 
g 2 = [“1 0 0] r meters 
g 3 = [1 0 0] T meters 
g 4 = [0 0 1 ] T meters 

(5) m 0 — 410 kg 

TYix = 6.8 kg 

m, — 57.5 kg 
m 3 = 0 
m 4 = 10.7 kg 


14 


JPL TECHNICAL REPORT 32-1592 




This completes the list of constants that arc needed to characterize the system. 
It remains then to develop expressions for the dynamic forcing functions T* F k , 
and r*. 

Control of the vehicle shown in Fig. 4 is to be maintained by firing pairs of gas 
jets located on the reference body to produce, ideally, a pure torque couple on that 
body. It will be assumed in the simulations to follow that this is the case and that 
no net force is applied to any of the system bodies; i.e., F k *= 0 for The con- 
trol torque T° can be expressed by 


p - [L,u uu uuv 

where the control functions f u and f 4 each may take on the values zhl or 0, 
depending on the single-axis control law defined in Fig. 5. (All other T k for ke£P 
are assumed to be zero.) The control law is based on position feedback signals 
derived from celestial sensors (i.e., sun sensors, star trackers) as well as rate feed- 
back derived from gyro measurements. If the sum of these feedback errors is 



Fig. 4. A space vehicle Idealized as a system of five hinge-connected rigid bodies 
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greater than some arbitrary value (±9db in Fig. 5), a switching amplifier fires the 
gas jets to apply corrective torques. A list of values assigned to the reference body 
attitude control parameters follows: 

6 db = 0.005 rad L t = 0.23 n-m 

tr — 2.0 s L 2 = 0.21 n-m 

= 0.31 n-m 
TM = 0.020 s 

Finally, it is necessary to characterize the hinge torques T k (k€£P)> Since the 
boom will not be articulated, the boom hinge torques can be described by 

T3 == ~Kzy$ B$y 3 
T 4 " - KlY<l ~ 

where 

Kj — 2000 n-m/rad, B* = 10 n-m-s/rad 

K t = 2000 n-m/rad, B 4 — 10 n-m-s/rad 

However, since the TV platform will be articulated relative to its nominal point- 
ing direction, it is convenient to introduce the variables y ic and y 2 c to represent the 
commanded position of the platform. The hinge torques can then be written as 

Ti = — K t (yi — y ic ) ~ B 1 y a 
t<> = — Kv (y 2 — y 2C ) — B 2 : /2 

where 

Ki — 3500 n-m/rad, B x = 20 n-m-s/rad 

K 2 = 3500 n-m/rad, B* — 20 n-m-s/rad 

J # Case 1— pfof/orm ttuzneueer. The first simulation of the five-body sys- 
tem of Fig. 4 and its controller will show its dynamic response to a large platform 


SWITCHING AMPLIFIER 
(MIN I MUM-ON-TIME ^ TM) 



Fig. 5. Single-axis attitude control loop 
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maneuver. Initially, the platform is positioned at y t — 218 deg and y 2 ~ —30 deg. 
All other system variables, i.e., angular positions and rates, are assumed initially at 
zero. At t = 0, the command is issued to rotate the platform (about both axes simul- 
taneously) at the constant rate of 1 deg/s to the position y L = 168 deg, y* = 20 deg. 
The program used to simulate this maneuver is shown in Fig, 6. 

Note that the required MBDY specification statement REAL MB(7), . . . has 
been replaced by its equivalent in the CSSL III language, i.e., ARRAY MB(7), . . . 

Also, note that the HCK routine mentioned in Section I is used to establish the 
position of the reference body in the inertially fixed frame, and it thereby provides 
a means for calculating 6 a , 0 2 , and 0 3 , the attitude angles measured by celestial sen- 
sors fixed to the reference body. 

Three other CSSL III built-in functions are also used in the simulation. The func- 
tion “SWAMP” simulates the switching amplifier shown in Fig. 5, “STEP” provides 
the unit step function when the independent variable, TIME, is greater than the 
specified constant. And finally, “INTEG(ai,a 2 )” signifies the integration operation, 
where is the integrand and a 2 is the initial condition. 

All arithmetic statements are in Fortran, although CSSL III allows several state- 
ments on a single line if separated by a ” Variables to be plotted at every com- 
munication interval Cl are listed in the PREPAR statement. Printed variables are 
listed in the OUTPUT statement 

The statement “CALL MBDY(NH, . . .)” is located in the INITIAL section and 
is therefore executed only once, i.e., prior to the dynamic calculations. However, 
“CALL MRATE(NH, . . .)” is in the DERIVATIVE section and is thus executed 
at every integration step, as required. 

Results of the simulation described in Fig. 6 are shown in the computer plots of 
Fig. 7. All variables plotted are in terms of radians, radians per second, and seconds. 

Notice that all three components (Wl, W2, W3) of the reference body angular 
velocity o>°, show evidence of platform hinge vibrations immediately following 
platform acceleration or deceleration (i.e., at t — 0 and t = 50 s). Only Wl and W3 
show any significant evidence of the lower-frequency boom vibrations, since the 
boom is nominally aligned with the reference body's “2” axis. Attitude angles 9 ly 0 2 , 
and 0 3 (THET1, THET2, THET3) have obviously been effectively confined to the 
±0.005-rad deadband by the control system. Platform and boom angular rate and 
position responses clearly show the transients at t — 0 and t = 50 s. Some platform 
vibration is discernible in the boom responses. 

Figure 8 repeats the transient solutions for Wl, W2, and W3 over just the first 
10 s in order to display the response in better detail. The W2 response indicates 
that control jets began pulsing at about t — 4.6 s in that axis, while pulsing in the 
other axes was not required until much later. 

2. Case 1 with prescribed motion . In order to demonstrate the utility of the 
MBDY subroutine's option for prescribed motion. Case 1 was rerun with the plat- 
form rotations yi and y 2 prescribed. As indicated in Section IIE, the motion of any 
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PROGRAM 5-H00 Y SYS TEH SIMULATION U$JNC, H^DY --- CASE 1 
INTEGER N H , H I I'll ,hOJ 

ARRAY MB<7>,MAl4>7>tFB< < t>JJiPAC4»4>3j l G<H*3»»Tbm,TA<4*3i 
ARRAY FH(3},fA(4,3) f THlH^,CirMHJ , GMU U 1 , GMOQ < H ) , AG I 3 I 
DOUBLE PRECISION flOOTlyJ 
data H/OtifOta/Pi/OjOjO.a/ 

OATA 

DATA MA(ttI>/«3^/MAU,2)/#35/NA(i*/)/6*a/ 

data MA<2 f p/4*Bb/riAiZt2)/2*2/MA(2i3)/S t 5/MA(2 l *f)/.,Hj/MAc7m>/pLr// 

DATA wA(2»&>/-.&*i/MA<2 P 7)/ 5 7 # b/ 

DATA MAt<it!>/27*2/HA|4f l 2)/ v 2/MA(H«3)/27»2/MA(«ii7)/10*7/ 

DArA P0 U»3I/«-1 »s/PH(3i2)/*I.2/ 

DATA PAll,»,3>/-,7S/PAll,2,2j/-,lC/PAll l 2, 3 )/-./5/ 

DATA PA(2,2i2)/-«22/PA<2,2,3)/«20/ 

data Pa14,4,2)/3*3/ 

DATA 6U g 3)/|./e<2 f n/-|./6C3tU/U/6CH # 3l/U/ 

CONSTANT T^a f G2o,06*>fOC&»TR B 2*0 
CONSTANT TFiNALa60 #t cLKTlH e liQ0* 

CONSTANT LI *#23 ,L2= .2 I p L3* * 3 i 
CONSTANT SLKAT*,ai7^S329 
CONSTANT PhU«Q» *THETZ*0« *pS|Z*Q. 

CONSTANT *1 1>Q » |%2 l”C' 

CONSTANT Kl»3SCCMK2 3 3500,tN3*2QCQ.«< t + = 2 00 0- 
CONSTANT ‘*l=2O*it}2a2c*.03alO*»B4«lO* 

CONS T ANT l c 1 * 2 1 8 * , I C 2 * - 3 0 , 

INITIAL 

NH*4 

RDsS7#29S7 7?5 4 DK«U/RD 5 GMlCl*Id/RO * GM2CI»IC2/RD 

GHll»IC!/«D 5 <*M2 1 » J C2/W0 

CALL MBOYlNHgH , M B , M A , P f* , P A ,^P!} 

PZ!,PAl f PBt v PCl-HCKl]NiTL,PH|Z,THE1* v PSM) 

END 

dynamic 

1F< TIHE^GttTFINAU Go tQ Si 
DERIVATIVE BOOYb 

VARIABLE T1ME»C. 

CINTERVAL Cla.Qi 

XE^ROR rt|»i^E-6 f VK2 = I,E:^6»A3«J«E-A f 6Mj*i # E-H 
STPCLK CtKTiM 

OUTPUT 50, AID »K2D t *30 . W 1 ,W2 f W3 ,THET j , T HE T 2 • T H t T3 , • - « 
iNi#lN2ilN3iGHl » G M 2 1 gH 3 , &M 4 ■ PL 1 * P l 2 
P REP A R tv l ,a2 • A3 , THE! 1 iTHEt2»THET3,CM|0,Gm2d, < t « 

GM 1 »GM2 >Gm 3D i^M40 ,GM3 

NOSORT 

5 ^ 0 l ^ * V* 2 5 v»Q<3>**3 
PLI*RD*GMl 5 PL2 «RD*gH2 

$ GMZ>«GM2 % GHI3)bG^3 S 

GMO(n»GHlD % GMD( 2 )nGN 20 $ G M U I j ) o <3 M 3 D S G MO / *n »GM <40 
COMMENT 

COMMENT CElEST I AL^SENSOR-MEASuRED angles 

comment 

01 |D2*HCK(MATRIX|P2Q*PaQ*PB0|PCG) 

NKO,NYo,NZo«HC^( iTOtt»0, |0 M T* »DI i02i 
txo ,LYO ,L2o»HCK ( l TOB , i f ,0* f 0t $ 0 | , 02 i 

THETiaATANitNYUjNZO) % THET2»ATaN2 \ -»NXO 1 N Z 0 ) 
THET3»aTAN2<-L y 0pL*0) 


Fig, 6. Five-body system simulation using MBDY (Case 1) 
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S GM2C>iNTEG<GM2C0'<ii"2Cl I 


T« i 3 >*L3+AMp3 


COMMENT 

COMMENT HaTL FEEDBACK SIGNAL and SWITCHING amp* DYNAMIC 4 -* 

COMMENT 

INI*"ThET1-TR*M 5 In2-*THET2-TR*W? S \ N3»^TH E T T ft* * 3 
AMPINS (T I M E »TM|Ub»]N1 > 

AmP2*5v*AMP (T IME|TMtUfift|N2| 

AMP3*S*AMP<TJMt,TM#l>8t|N3> 

comment 

COMMENT PLATFORM POSITION COMMANDS 

comment 

CMUO*SLRAT*(-U+STEp<bO*OfTIMEn S GMIC = INTEGU»MlCn*GMlC]> 
GM2CO*SLR A T*<l»-STEP(S0iiTlME>> S GN2C>SNTEG(GH2COt«M2Cll 

COMMENT 

COMMENT REFERENCE BODY EXTERNAL TORtiVJt 

COMMENT HINGE TOROOES 

COMMENT 

TB ( 1 )«Li*AMP| $ T6 (2 3 »L2*AMP2 $ yS < 3 > a L 3 *AMP3 
TH< n*-K I ♦ I GM1 ^GMl C >-Bl *GM 10 
THI2>«-K2MGM2*GM2C)-B2*GM2D 
TH( 3 ) *-K3*GM3- b 3*GM3D 
TH< t O»-KS*GMM- t3 H*GMSD 
COMMENT 

COMMENT SYSTEM ANGULAR ACCELERATIONS 

comment 

Call MKATt INH,1 h, TB,TA f f B t F A > GM » GM U , gMD D »«0 i*DOT I 
ftJD»*OOT(l) 4 W2D«^D0T(2) * to 3 DwlfiDP T I 3 > 

COMMENT 

COMMENT SYSTEM ANGULAR RaTeS and POSITIONS 
COMMENT 

YV)«1NTEG*W0QT* I > * to l I ) 
to2*lNT£G<WD0TU) i«21 ) 

A‘3*INT£G<WD0TI3> ,#31) 

GMlO»lNTEG;i(VDOT (H I » Q • I $ GM2U*lNTEG<toD0T<5f.a.) 

GM30* INTE^I WOOT { ,0. > i 6M«f iNTLGltoOQT ( 7) » Q V ) 

GMi*lNTEGUMtO»GMlI) $ GM2*InTEGIGmZD,gM2U 
GM3«lNTEG(GM3DfC*l $ SM^JNTEGlGl^DiOH 
COMMENT 

COMMENT HC K parameter rates and positions 
COMMENT 

PZOD |PAOO tPBOO t PCOO*MCK^ HCK |P20 |PAO tPBO tPCo tW I f W2 I 
P20*INTEGtpZOD»p2l ) 

P AO* INTEG<PAOO »P A I I 
PBO«INTE6tPBOOiPBj) 

PCO*INTEG<PCQD»PCI ) 

END 

END 

END 

TERMINAL 

SI** CONTINUE 

END 

END 


S GM20*lNTEQ(«fD0T<5Na^ 

S GM40*iNTEGUDQT< 7) tC,') 

GM2* InTEG l G m Z D . G M 2 l J 
GM*t« INTEG (GhRD iO* ) 


Fig. 6 (contd) 
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Fig. 7. Results of Case 1 simulation using MBDY 
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Fig. 7 (contd) 
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Fig. 7 (contd) 
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THET1, 0,, THET2. 0„, THET3 



Tig. 7 (contd) 
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Fig. 7 (contd) 


24 


JPL TECHNICAL REPORT 32-1592 



GM1, Yy 



Fig. 7 (contd) 
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Fig. 7 (contd) 
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Fig. 7 (contd) 
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Fig. 7 (contd) 
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Fig. 7 (contd) 
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Fig. 8 (eontd) 


32 


JPL TECHNICAL REPORT 32-1592 


body 6i, (ie<9) may be prescribed relative to an adjoining body 6f, where / = hi 
(Def. 37), by setting PI(t) = 1 and supplying the appropriate values or function 
for yi(GMDD(t)). This was done for the platform bodies h and resulting in 
the simulation program of Fig. 9. Notice that the platform command angles have 
been replaced by time functions for GMDD(l) and GMDD(2) (using the CSSL III 
"RAMP” function). The GMDD(l) function is plotted in Fig. 10. 

The expressions for the hinge torques tj. and r 2 (TH(1), TH(2)) have also been 
removed from the original program (although it was not necessary) to emphasize 
the fact that they are not used when y x and y 2 are prescribed. 

Figure 11 shows the simulation results. It is clear that the overall system response 
is virtually the same as that obtained from the first simulation (Fig. 7), except for 
the absence of the characteristic platform hinge vibrations. Since the simulation 
with prescribed platform motion took only min of computation time, compared 
with 7 % min for the first run, the usefulness of the prescribed motion option is well 
demonstrated. In this case, not only was the order of the system equation reduced, 
but the absence of the high-frequency platform vibrations helped to speed up the 
numerical integration process. 

Of course, some caution must also be exercised when employing prescribed vari- 
ables to make sure that the dynamics being eliminated are not important. In the 
sample problem considered here, the platform hinge vibrations could well have 
triggered gas jet firings before damping out if t r had been larger and/or the initial 
conditions had been different. The absence of these vibrations might then have 
resulted in an entirely different time history of the reference body attitude angles. 

One additional simulation of the Case 1 platform maneuver was performed 
without the application of control torques. In the absence of external torques, the 
system angular velocities should return to their initial values (i.e., zero) after the 
platform maneuver is completed (Conservation of Angular Momentum). Of par- 
ticular interest are the reference body angular rates, which are given in Fig. 12. 
Although vibrations are still in evidence at t = 60 s, it is clear that the final average 
rates are zero, as they should be. (The small high-frequency disturbances which 
seem to occur randomly, particularly in the W2 and W3 responses, are due to 
numerical integration errors.) 

3 . Case 2— small platform maneuver ♦ The subroutine MBDY, since it solves the 
complete and exact set of equations of motion, must certainly be used when rela- 
tively large angles and angular rates are expected at each hinge. However, if the 
hinge rotations or both the rotations and rotational rates are ‘small” at any hinge, 
some linearization of the equations of motion may be effected, often to good advan- 
tage in simplifying and speeding the computational task. 


In subsequent sections of this report, modified versions of MBDY, employing 
some form of linearization, will be described. One version, MBDYTL, employs 
total linearization; i.e., all angular position and velocity responses in the system 
are assumed "small” in some sense. A sample simulation, using the five-body con- 
figuration of Fig. 4, will be performed later using MBDYTL, and it is therefore of 
interest to have for comparison purposes an "exact” solution to the same problem 
using MBDY. 
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PROGRAM 5-80BY S YS TEM SIMULATION USING MB D Y --- CASE 1 
INTEGER NH.PH41.HU) 

ARRAY HBI 71 »NA (4.7 I .P0 U .3) .PA 14 .4 . 3) .G 14 .3 ) . TB 13 ) . t A|4, 3 | 
ARRAY F8miFAI4.31.THI4>.GMI4l«3HDI4|,GMDDI4>.W0t3) 

DOUBLE PRECISICN WDOT ( 7 1 
DATA H/a* I >0» 3/PI/I #1 .0.0/ 

DATA MB/Ii5»»316.*44C,,14,»-14.,34,6»410,/ 

DATA MAT! . 1J/. 35/MAtl • 2 ) / , 35 /M A < l , 7 ) 7 6 . 8/ 

DATA M A ( 2 , 1 ) / 4 • B 5 / M A (2.2 l /2 . 2 / M A I 2.51/5 ,5/MA 12.4) 7 - , 4 1 7 m A (2.5 ) / ,07/ 
DATA MA (2 .6 I /- .54/MAI 2, 7 )/ 5 7 .5/ 

DATA MAT 4.1) 727.27KA 14.2 )/.Z/KAI4 .31 7 2 2.2/MA U. 7) / IQ , 7 7 
DATA PS 1 1 . 3 I /- 1. 57PB1 3. 2 ) 7- 1 .2/ 

DATA PA I 1, 3 , 3J/-.75/FAU .2.21 / - .1 0/P A 11,2,3 I ) - .75/ 

DATA PAt2,2iZ)/-.22/PA(2i2»3l7«2D7 
DATA PA ( 4 > 4 . Z I 73.3/ 

DATA Gil. 31/1. 7GI 2. 1) /- 1./ 313. 11/1. /GHi 31/1./ 

CONSTANT TM:.020.D8:.OB5.TR:2.0 
CONSTANT TFINAL : S 0 . . CL K T I M:4 2 0 . 

CONSTANT LI: .2 3»L2r ,21 ,L 3: . 31 
CONSTANT SLRAT: .01745329 
CONSTANT PHIZ:G. .THET Z:0 .»PSIZ=Q . 

CONSTANT H1I=0. tH2I = a. ,«3I = 0. 

CONSTANT Kl:35CD.iK2=3500.»K3s20DQ,.K4:20D0. 

CONSTANT Bl:20. ,62 = 20.. B3:13. ,34:10. 

CONSTANT IC1 :2 18 .. IC2S-30. 

INITIAL 

NH :4 

R D: 5 7 . 2 95 7 7 95 S Ofi=l./RQ S GM1CI=ICWR0 $ GM2CI:IC2/RD 

GM1I=IC1/RD I fM2I=IC2/RD 

CALL M0OYI NH.H. M0.MA .PB.PA.G.PI) 

PZI.PAI,PBI.PCI = HCK I IN I TL. PHIZ , T ME 1 Z.PSIZ ) 

END 

DYNAMIC 

IF t TIME. GE. IF INAL) GO TO SI 
DERIVATIVE B0DY5 

VARIABLE T I ME :Q , 

CINTERVAL Cl: .0 6 

XERROR Hl=l.E-6 *W2=l.E-6* W3=l .£-6,GM3:l .£- 4 
S1PCLK CLK1IM 

OUTPUT 5Q»WlD.W20»W3QiWI,W2.U3,THETl»IHEI2iIHEl3..., 

INI , IN2 .INS , GM 1 , GH2 .GM3. GM4 ,PL1 , PL 2 
PREPAR HI. U2. M3 « THE 11 . THE 12, T HE I 3 , GM ID. 0M2 D, . , , 

GUI .GM2.GM3D.GM4D.GM3.GM4 

NOS OR 7 

N 0 < 1 1 : HI t MCI 2 1 =H2 S NC(3)=H3 
PL1 :RD* GM1 $ PL2=RD»GM2 

GM 1 1 ) : GM 1 1 6M21=GM2 $ GM 1 3 1 : GM3 * GMI4I:GM4 

GM0I1):GM1D t GHOIZ):GM20 1 GMDI3):GM3D $ GMD(4|:GH4D 
COMMENT 

COMMENT CELEST IAL-S ENSCR-MEAS ORE D ANGLES 
COMMENT 

D1 »DZ:KCKIMA TR lX.PZC.PAC.PBO.PCO 1 
NXO. NYO. NZO:H CM (1108, 0..Q..1, .D1.D2) 
LX0»LY0.LZ0:HCt , (ITQ6.1.»0..U.>Dl»D2) 

TH£T1:aTAN2INyO.NZO) $ 7 HE T 2: A I A N2 I - NX 0, NZO I 

THE1 3: A TAN2 I -L YO .LXC I 


Fig. 9. Case 1 simulation program using MBOY, with prescribed platform motion 
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COMMENT 

COMMENT PSEUDC-RA1E FEEDBACK SIGNAL AND SWITCHING AMP- DYNAMICS 
COMMENT 

IN1 = ~TH£T 1 - I R * W 3 * IN 2 r- T HE T 2 - I R * W 2 5 I N3 = - T HE T 3- T R* U 3 
AMP1"5WAMP< TIM£flM»D8»INl ) 

AMP2 = SWAKPt TIMEi TMrOB t IN2I 
AMP Jr SWAMP ( TIME • TM*D9* IN 3 ) 

COMMENT 

COMMENT PRESCRIBED PLATFORM ACCELERATIONS 
COMMENT 

GHDD! 1>?6. 981316* f-ft4MPI0«t?lM£>*2«*RAMPJ.05' TIME!-.. • 

R AMP I . 1 » T IME !♦ RAKP |5Q • * T IME > -2 ,*R AMP t 50 .Ob» 1 IME > ♦RAKPTSO .10 »TIHE H] 
GMDD«2)=-GMDDI1 J 
COMMENT 

COMMENT REFERENCE BODY EXIERNAL TORQUE 
COMMENT HINGE TORQUES 
COMMENT 

Tail )=L1* AMP1 $ I E t 2 ) - L2 * AMP 2 $ 18 I3)-L3*AMP3 

THT 3 ) -- K 3* GM3-3 3*GM30 
TH (4 l-“K4 <GK4-E4*GH4D 
COMMENT 

COMMENT SYSTEM ANGUAR ACCELERATIONS 
COMMENT 

CALL MRAUINHt TH.lBiT AiF6»F A#GMt 8MD • GMDD »U0 *U00 T I 
W1 D r wDO I i 1 ) S W 2D - WDOl 121 $ W30 = WD0TI3) 

COMMENT 

comment system angular rates and positions 

COMMENT 

W1 -IN T E G < WOO T t 1 > .Will 
VJ2-INTEGT WD0TI2 > i W2I ) 

W3-INTEG ( UDO T I 3>»W3I) 

GMI D- 2 N T EG T WDOT <4)10.) 

GK3D-INTE G ( W DO 116 > * Q » I 
GM1-INTEGT GMIOi GM1IJ » 

GM3rINTEG <GH3D.0.1 $ 

COMMENT 

COMMENT HCK PARAMETER RATES 
COMMENT 

P 2 OD *P AOD >P B 0 D iPCOD-HCK I HCK i P2 0 i P A 0 iP B 0 f P CO t W1 • W2 > W 3 ) 
PZO=INTEG<PZQD.PZII 
P A 0- IN TE G IPAOD tPAI ) 

P50zINTEG<P30D#PBI) 

PC 0- IN TE 6 (PCOD .PCI \ 

END 
END 
END 

TERMINAL 
SI - . CONTINUE . 

END 
END 


$ GM2D-INIEG i WDO U 51 iO. I 

* GM40 = INTEGtWD0T 

GM2- IN I E G 1 GM 2D * GM 21 J 
GM4-IN7 EG f GM4D.0 . J 

AND POSITIONS 


Fig. 9 (contd) 
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In Case 1, we have already seen that the reference body and boom responses 
are relatively small even though the platform angular excursions are substantial 
If the platform maneuvers were also restricted to small values, this would seem 
the ideal case for which to apply MBDYTL and to obtain a check solution using 
MBDY. Results of the latter will be presented here. 


In this case, it will be assumed that the platform again starts from the position 
yi — 218 deg and y 2 = ~^30 deg and that at t — 0 the command is issued to rotate 
to yi = 220 deg at a commanded rate of 1 deg/s. Then, at t ~ 5 s, the command is 
issued to rotate about the other axis to the position y 2 — —25 deg, at the same rate. 
A CSSL III program to simulate this maneuver sequence is listed in Fig. 13. 


The simulation results are shown in Fig. 14. Obviously no gas jets were fired. 
These solutions will be compared in following sections to those obtained from 
linearized versions of MBDY. 


III. Totally Linearized Systems 

A. The Equations 

When any justification can be found for the linearization of any of the kine- 
matical variables appearing in Eq. (5), this step should be taken, since the mathe- 
matical and computational benefits of linearization are quite substantial. The 
primary immediate benefit is the elimination of variable terms from the coeffi- 
cient matrix on the left side of Eq. (5). If and all angles y 1} * * * , y„ and their time 
derivatives can be assumed to remain arbitrarily small, then, the indicated coeffi- 
cient matrix is formally constant, and much computer simulation time can be gained 
by computing its inverse only once, in advance of numerical integration. When the 
angles are expected to be small in an engineering sense, less rigorous arguments 
can be marshalled to gain the same computational advantage. However, more than 
sufficient reason for employing linearization can very often be found by experi- 
mental demonstration. 
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Fig. 11. Results of Case 1 simulation with prescribed platform motion 
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Fig. 11 (contd) 
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THET1 , e 1# THET2, 8 ~, THET3 



Ffg. 11 (contd) 
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Fig. 11 (contd) 
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GM2, y 9/ GM1 



Fig. 11 (contd) 


42 


JPL TECHNICAL REPORT 32-1592 



GM3D, y 3 



Fig. 11 (contd) 
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Fig. 11 (contd) 
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Fig. IX (contd) 
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Fig. 12 (contd) 
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Fig. 12 (contd) 
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PROGRAM S-EODY SYS1EF SIMULATION USING MBOY CASE 2 

INTEGER NH.PII41.HI4 » 

ARRAY MB< 1 1 »KA <4 >7 I .PB(4 .31 .PA 14 .4 .31 » G 14.3 1. 1 B <3 I. T AI4.31 
ARRAY FBI 31 t F A C 4 . 31 » T H 1 4 1 tGM < 4 1 a 3 H 0 I 4 1 * G H D D ( 4) • Irf 0 I 3 I 
DOUBLE PRECISItN U D C T 17 I 
DATA H/O.l. Ot 3 /P I / 0 »D »Q . 0/ 

DATA MB *115. . 316. .44C,. 14. ,-14. .34. 6 .410./ 

DATA MA4l.il/. 35 / M A H ,2 I / . 35 / M A 41.7 1/6.9/ 

DATA MAI2. 11/4. 85/MAI2. 21/2. 2/KAI2. 31/5, 5/MAt2.4»/-.41/MAI2*5)/. 07/ 
DATA MA4Z.6I/-.54/MAIZ.71/57.5/ 

DATA HA14.J1/27. 2/FA (4.2I/.2/NAI4.3I/2J.2/MA (4.11/10.?/ 

DATA P9I1.3I/-1.5/PB43.2I/-I.2/ 

DATA PAIl.lt3)/-.75/FAtl.2.2)/-.10/PAIlt2.3>/'.75/ 

DATA PA42.2.21/-.22/PAI 2. 2, 3 I/. 20/ 

DATA PA (4.4.21/3.3/ 

DATA G(1.31/l./St2ill/-l./Gt3>l)/]./GI4.3>/l./ 

CONSTANT TH=.020.DB=.GQ5.TR=2.0 
CONSTANT TfINAL =20. . CLKT IH = 1 1 CD. 

CONSTANT M = .23iL2=.ZliL3=.31 
CONSTANT SLR A T = .01745329 
CONSTANT PMIZsC. .THETZ=Q..PSIZ=0 . 

CONSTANT M1I=0. » U 21= 0. . W3 1=0 . 

CONSTANT Ml =35 CG..K 2=3500. *K3= 2000. iK4=2D0Q . 

CONSTANT 01 = ZD. . B 2 = 2 0. . B 3 = 1 0 . .34=10. 

CONSTANT IC1 =218. .IC2=-30. 

INITIAL 

NH = 4 

RO=57. 2957795 4 DR=1,/RD $ GMlClslCl/RO 4 GM2CI=IC2/RD 
6H1ISIC1/RD * CM2I= JC2/R0 
CALL HBDT4NH.HfMB.MA.P8iPA.GtPl) 
PZI.PAI.PBI>PCI=HCK(IM1L*PH12.THET2«PSIZ1 

END 

DINAMIC 

IFCTIME.GE.TFINAL1 GO TO SI 
DERIVATIVE' B0DY5 

VARIABLE T IME =0 . 

CINURVAL CI=.0? 

A ERROR Wl=l.E-6.W2 = l.£-G«W3=l.E-G»GMJ = l.E-4 
S1PCLK CLKTIA 

OUTPUT 50. MID. N2 0.U3D.U1 .U2. 83. THE1J .THE 1 2. THE! 3. .. . 

IN1.IN2.IN3.6M1. 6H2 .GM3.GH4.PL 1.PL2 
PREPAR U1.N2.U3.THET1. I HE T 2 . T HE 1 3 . GM 1 D . GM2 D . ... 

GM « G F 2 .GK30.GH4 0.GM3 .GH 4 

NOSOR1 

U0(I> = U1 * U 0 ( 2 I =M2 S MC 13 I =M3 
PL1 =R0* GM1 t PL 2=fi D* GH 2 

GM (1 > = GH1 t £FI2I=GM2 t GH|3)=GH3 t GMI4)=G«4 
GMBm = GH10 4 GMD( 2 1 =GM 20 4 £MDI3I = GK30 4 G MQ< 4 4 =GK4 D 

COHMENT 

COMMENT CELESTIAL-SENSOR-HEASURED ANGLES 
COMMENT 

D1 .02 = PCK IMATR I X .P ZC . PA C »P B C .P CO 1 
NXO*NVO.NZO=HCK< IfOB.O. >0. .1. .01.021 
LX0rLTC.L2O = HCHIT0B.l>.0..0..D1.02l 
THET1 = »TAN2INYO.NZO) 4 I HE T 2 = A T A N2(-N XO . NZ 0 1 
THET3=ATAN2( -L TO.LXCI 






'COMMENT 

COMMENT RATE FEEDBACK SIGNAL AND SWITCHING AMP. DYNAMICS 
COMMENT 

IN1 = -TH£T1-TR«U * IN 2 T HE 1 2- TR » W2 S I N3S- T HE 7 3- T R« W3 
4MPI = SU'AMP< 1 1 ME «TM. 0 9. IN 3 i 
AMF2:SWAHPt TIME. TM.OB.INZJ 
AMP3SSWANPT TIME *TM«OB*INJ) 

COMMENT 

COMMENT PLATFORM POSITION COMMANDS 
COMMENT 

GMT CO=SLRA!»< I.-STEPI2..T IME M i GHlC-INT£GfGMlCD.OMlCI> 

GK2CD=SLR AT*<S TEF 15 . i T IK £> -5 TE P 110 . . T IME 1 1* GM2C = INT EG I 8M2 CD »GM2 C I I 
'COMMENT 

COMMENT REFERENCE ECDY EXTERNAL TORSUE 
'COMMENT HINGE TOROUES 
'COMMENT 

i TBI 1J =L1*AMP1 S T9I2T-L 2*AMP2 % T B 1 3) =L 3 • A HP 3 

' IHtl 7 = -Kl •IGK1 -6HC>-B1»GM1D 

TH<2IS-K2MGH2-GM2C>-B2»GH2Q , 

TH 1 3 I - -K3»6M3- E3«GM3D 
TH I m s- K4* GM4-0 4«GM<I0 
COMMENT 

COMMENT SYSTEM ANGULAR ACCELERATIONS 
COMMENT 

CALL HRAT£<NH<THt!B«TA<F8 »FA»’GH»GMO»GHDD«WO« WDOTI 
U1D:WD0T 111 * L20=W00T»2I S W3D=UDGTI3I 
COMMENT 

COMMENT SYSTEM AN6LLAR RATES AND POSITIONS 
COMMENT 

W1 =INIEG( WDOU 1 » «W1I) 

U2=INI£GIWD0Ti2l«W2I) 

U3-INlES(UOOTI 3T.W3K 

6 Ml 0=ZNTEGI MOOT I 4 I • 0. I 3 GM2D = INT EG T HD OT « S J i G. I ' 

GK3D = INTEG«NG0 ItCTtO. I t G M4 D : IN T EG 1W D0 1 t 7 I . 0 . I ' 

6M1 =1 NTEG1 OHIO* GMlI > J GM2 = IN TE G < GHZO t GM2I I 1 

GM3 = IN1EG (GM3D iO .1 t GM4 = INT EG I GM4 D.O .) 

COMMENT 

COMMENT KCK PARAMETER RATES AND POSITIONS 
COMMENT 

PZCD»PAOO*PBOD iFCCO-HCKt MCK tPZO . PAO «F B 0 1 P CO »U 1 * K Z t N 3 l 

PZO=INTEG(PZOD»PZIJ 

PAO: IN TEG IPAOO iPAI I 

P80=INTEGYPBGD.PBII 

PCC = INTEG IPCCD .PCI » 

END 

END 

END 

TERMINAL 

SI.. CONTINUE 

ENO 

END 


Fig. 13 (contd) 
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Fig. 14 (contd) 
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TIME 

Fig, 14 (contd) 
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Fig. 14 (contd) 
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Fig. 14 (contd) 
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In any case, Eqs. (5) can be rewritten in the form 


and 


tfoow 0 4* 2 tfofcy*; — 2 -Eft 
k€&> 


(8a) 


aiow 0 4- 2 tf-ifcYfc = g tT C t0 2 E ik Ei c + ri (8b) 

k€@ ke@ 

The next step is the linearization of Eq. (8) in the variables o>°, y* and their time 
derivatives. To this end* we expand each symbol in Eq. (8) which may involve such 
variables into three parts, the first being free of these variables (indicated by over- 
bar), the second being linear in the variables (indicated by overcaret), and the third 
containing terms above the first degree in the variables (indicated by three dots). 
In particular, we write (for any % k) 


QiO = CiO + CiO + ... 

(9a) 

T a = Tfl + T<* + ' • * 

(9b) 

a = 4 4- ' • ' 

— /N 

(9c) 

Ek ~ Eh + Ek 4- • * * 

(9d) 


and then determine explicit expressions for the new barred and careted symbols 
from the definitions of a i}c (Defs. 32, 34, 35), E k (Def, 36), and the expansion for C in 
(Appendix A). In terms of the symbols introduced in Eq. (9), the linearized form of 
Eq. (8) becomes 

5oow° + 2 ttokYh = 2 (Ek + E k ) (10a) 


(i € £?) a iQ »° + 2 dikyk -g* 2 E ik [C« (E k + E k ) + C i0 E k ] + u + ?i 

kt& ke&> 


(10b) 

In deriving explicit expressions for C i0 and C i0 , we may begin by recognizing 
that for any CN, where r and j are labels of adjacent bodies, 


and 


C rj = C* r — U = 3 X 3 identity matrix 

c r} = -y.gr, (■ r>i ) 

C 3> = y r g r = (C^) 7 


It is clear that C i0 must then be given by 


C i0 - -2 *sig*7*> C oi - (C i0 ) T = 2 e 8i g*y„ 

se& s€& 


Also, C i0 = 17. In general. 


and 


(r, / e£8) C ri — 2 {**> ~ E ar )g a y s 


C r * = u 


(11a) 

(lib) 
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(12a) 


From Def, 36, E* and E k are given by 
Ek - C ofc [f fc + 2 D hi C ki Fi] ~T k - 1-2 D* j F J ' 

/e& 

£* = C 0 *[T* + 2 D*'6*'F' + 2 D^C^F'] + C ok [T k + 2 D^'C fc/ F y ] 

__ _ WB 

= T k + 2 D*'C^F' + 2 D*'F' + C°* [?* + 2 D*'F'] (12b) 

je£B ?«63 f«6B 

Of course, the most important result of the complete linearization is the absence 
of any time-varying elements in a 0 c, a 0 1 , 5io, and a^. These constant coefiicients now 
allow the square matrix on the left side of (5) to be inverted once and for all, in 
advance of numerical integration. 

It would remain then to determine T fe , T k , F* F k , t*, and for the specific system 
under study and, together with the relations in (11) and (12), to carry out the com- 
putations implied by (10), However, in constructing a subroutine to perform these 
computations, it was found to be much easier to directly manipulate the com- 
bined form: 

#on^° T 2 ttokyk ” 2 E k (13a) 

k*£P 

5io«° + 2 — g tT C t0 2 ci k E k + Ti (i« tSP) (13b) 

where 

I fc - [f fc + 2 5*5*^] 

c i0 - C i0 + C i0 

gofc = c ok + c ofc 

^ — XV 

_ yfc 

F> 

- — - j. ^ 

Ti — Ti ~r Ti 

A A 

By dealing only with the variables £*, C OJc , etc., rather than separating them into 
the parts E ky E k> etc., and explicitly forming all the separate terms in (10) and (12), 
the computation becomes much more straightforward and efficient even though 
some second-order terms in the linearized variables are retained. 

One further form of the system equations, which goes beyond strict linearization 
in order to simplify and speed the computations, is the following: 

(loon 0 + 2 G-oky* ~ 2 E k (14a) 

ke&> 

aio<l>° + 2 = g lT 2 £ikE k + Ti (14b) 

kt£P k€&> 

where 

EJ = C<* [T k + 2 D*'C*'F'] = T k + 2 

i fc£B 

Equations (14) obviate the computation of any body-to-body coordinate trans- 
formation matrices, C r; , at all. 
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Subroutines which provide solutions to both Eqs. (13) and (14) will be described 
in the next section. 

B. Subroutines MBDYTL and MBDYL 

The subroutines MBDYTL and MBDYL provide the solutions to Eqs. (13) 
and (14) of the preceding section, respectively. As in the case of MBDY, they each 
may be exercised by either of two call statements, the first of which initializes the 
program with the system constants. 

Initializing Call Statements 

CALL MBDYTL(NH,H,MB,MA,PB ? PA,G,PI) 

or 

CALL MBDYL(NH,H,MB,MA,PB,PA,G,PI) 

Except for the call statement name, the statements are exactly the same as that for 
MBDY. All the arguments arc defined exactly as given in Section IIE. 

To compute the angular accelerations o»°, y 1? • • * , y n the subroutine MBDYTL 
(or MBDYL) must be entered at every integration interval This is accomplished 
by executing the dynamic call statement. 

Dynamic Call Statement 

CALL MRATE(NH,TH,TB,TA,FB,FA > CM,CMD,GMDD,WO ) WDOT) 

Again, the dynamic call statement arguments are the same as those defined for 
MBDY in Section IIE, with the following exceptions: 

TH(i) — array of hinge torques,?*; i 

A 

TB(/) 1 = array of elements of T° ; j = 1,2, 3 
TA(r,/) = array of elements of T 1 ; i e j = 1, 2, 3 

A 

FB(;) = array of elements of F°; j = 1, 2, 3 
FA (f,/) = array of elements of F l ; i e j = 1, 2, 3 

Even though the arguments GMD and WO are not used by either MBDYTL or 
MBDYL, and GM is not used by MBDYL, these "dummy” arguments are retained 
in the call statements for the sake of uniformity and to allow easy interchange of 
subroutines if desired. 

The Fortran "type* and storage allocation statements which must appear in the 
main program that calls MBDYTL (or MBDYL) are identical to those required 
for MBDY. 

Required Specification Statements 

INTEGER NH, H(n), PI(n) 

REAL MB (7), MA(n,7), PB(n,3), PA(n,n,3), G(n,3), TH(n), 

TB(3), TA(n,3), FB(3), FA(n,3), GM(n), GMD(n), 
GMDD(n), WO(3) 

DOUBLE PRECISION WDOT(n + 3) 
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Also, the statement 


PARAMETER Q = n 

must appear within the subroutine MBDYTL (or MBDYL) to allocate the proper 
amount of internal storage. Complete listings of the MBDYTL and MBDYL pro- 
grams are given in Appendix B. 


C. Application of MBDYTL and MBDYL to the Five-Body System 

I. Small platform maneuver simulation. We may now proceed to apply the lin- 
earized subroutines to the same vehicle dynamies/eontrol configuration examined 
in Section IIF. The vehicle is shown in Fig. 4 and the control system in Fig. 5. 
Expressions previously given for T°, r u t 2 , t 3 , and r 4 may now be re-interpreted 
as describing T°, r l9 ? 2 , r-*, and r 4 , respectively, since they contain no second- 
order or higher terms in the variables y* and their derivatives. In fact, in order 
to again simulate Case 2, i.e., the small platform maneuver from yi = 218 deg to 
y t = 220 deg (at t = 0) and y 2 — —30 deg to y 2 = — 25 deg (at t — 5 s), it is neces- 
sary to replace the statement "CALL MBDY(NH ) . . .)” with the statement "CALL 
MBDYTL(NH, . . .)” and to modify the arrays MA(2,i), PA(1,2,/), PA(2,2,/), and 
G(2 ,/). 


The reason for the required modifications to these arrays stems from Defs. 4 
and 12, which require all local, body-fixed coordinate frames to coincide (i.c., 
b* ~ bj for a = 1, 2, 3 and k, j e£B) when the system is in its nominal state, i.e., when 
yk — 0 for all ke&. When the routines MBDYTL and MBDYL are used, the vari- 
ables y fc are, by definition, required to remain very close to the nominal state y k = 0. 
Otherwise, the linearization process is meaningless. It is clear, then, that to simu- 
late Case 2 with the linearized programs, the new linearized variables %(k = 1,4) 
must be redefined in terms of the old y k such that, for example, 


?i = yi ” 219 deg 
% ~ yi + 2.7.5 deg 


74 — y 4 


And now it is also clear that bodies and & z have different orientations in their 
own body-fixed frames, implying difiFerent inertia matrices P, F and different com- 
ponents for the vectors p 10 , p 12 , p 21 , g 1 , g 2 in then respective frames, {b 1 } and {b 2 }. 

In this particular vehicle configuration, the platform hub, 4 1? has symmetrical 
mass properties about its axis of rotation, which in turn is aligned to one of the 
body-fixed coordinate axes. Thus, I 1 , and therefore MA(1, /), / = 1 — -7, remains 
unchanged, as does p 10 (PA(1,1 ,/), / = 1 — 3) and g 1 (G(l,/),/ = 1 — 3), However, 
it is apparent that p 12 , p 21 , as well as g 2 , and P must change, since they were previ- 
ously defined for yi = 0, y 2 = 0 and now must reflect the configuration defined by 
y* = 0, y 2 = 0 (y 2 = 219 deg, y, = - 27.5 deg). 
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The components of a vector described in the old frame fixed in can be related 
to its components in the new frame by the sequence of rotations 71 and 72, as 
follows: 



~C 7 j 

— syiCyt 

— Sy x Sy 2 


K) 

syi 

CyiCy-i 

CyiSy-i 

<«*? 

= C xo <d5> 

_ 0 

-Sy 2 

Cy 2 J 

M 

M 


(■ sy 1 = sin y lt c yi = cos 73, etc.) 


Therefore, the components of g 2 in a simulation linearized about 73 = 219, 72 = 
—27.5 deg become: 


e (-l) (-Cyi) (0.77715) 

= C K0 <j 0 J = | -S73 1 1 0.62932 V 

Similarly, the components of p 21 become 

( 0) ( 0.22 Sy,c y , - 0.20 Sy,^, ) (-0.18092) 

= C" vn ) - 0.22 > = <J 0.20 Cy,s r . - 0.22 c- n c ri > = ) 0.22342 > 

I 0.20 1 ( 0.22 s r , 1- 0.20 C y, } ( 0.07582 ) 

The vector p 12 only changes due to the 73 rotation, i.e., 



Pi* 
pi 2 


Cyi 

Syi 

0 


- .Vy, 0 

Cyi 0 

0 ij 


0 

- 0.10 

-0.75 


0A$ yi ) ( - 0.06293) 

-0.1 c yi \ = 1 0.07772 > 

-0.75 \ (-0.75 ) 


Finally, the modified value of P is given by 



■ 4.85 

0.41 

-0.07“ 


“ 3.52 

1.25 

-0.75“ 

P = C N0 

0.41 

2.20 

0.54 

( C yo)r = 

1.25 

3.79 

0.73 


_ — 0.07 

0.54 

5.50 


-0.75 

0.73 

5.24 


Having determined these changes in the system configuration description, the 
appropriate values of MA(2,i), i = 1 — 6, PA(1,2,/), PA (2,2,/), G(2,/) may be entered 
to produce the simulation program of Fig. 15. Notice in Fig. 15 that it was also 
necessary to change ZC1 and IC2 to reflect proper initial values of the new vari- 
ables 9i and y 2 (i.e. ? yi (0) — — 1 deg, y 2 (0) = —2.5 deg; these variables correspond 
to y x (0) = 218 deg, y 2 (0) = —30 deg). Using this program and the MBDYTL sub- 
routine, Case 2 was simulated again, and the results are shown in Fig. 16. They 
may be directly compared to the dynamic responses of Fig. 14. While there are 
some very slight differences visible in the angular rate responses, the overall result 
is virtually identical with that obtained using MBDY, and is obtained in one third 
the time used by MBDY (MBDY used 140 s, MBDYTL 48 s on the Univac 1108). 
Further, the substitution of MBDYL for MBDYTL gives exactly the same results 
as those shown in Fig. 16, since no external forces or torques were applied to the 
system, and calculates them in only 42 s. 


66 


JPL TECHNICAL REPORT 32-1592 



Obviously, the gain in computational speed when the linearized routines are used 
is quite significant, even for this relatively simple example. In Ref. 7, a 17-degree- 
of-freedom system is simulated using MBDYTL exclusively, since it gave a speed 
advantage over MBDY of about 25; 1 with more than adequate solution accuracy. 


PROGRAM s-mnv S V S T E ^ SIMULATION USING M30Y7L CASE l 

INTEGER NHfFinf . H I << I 

AR? A Y M 7 1 iM A1 4«71 3) «TR«3)«I •1^1*31 

ARRAY FS(3)»FM4.3) i^NmtGKmiSHOlWhGMDO^l^OU! 


DO U 91 E PRECISION WDOT I 7) 

DATA M/L.»l.ilit3/PI/D*0#L‘i0* 

DATA M H f 1 t 5 • • 3 ! « • * 'n 0 ♦ * 1 * . i - I * • t A s . 6 i « 1 0 . / 

0 A ' A H A t 1 • i) / . 3 5 / K A (1 • ? \t . 2 5 / V. A 11 t ? \ / o . & / 

DATA HAt?«n/3«5?/KAt 2t2l/J.7:T/MAl2.3|/5iZ<»/HA<2i<IW-4.25/MA12.5l/.75/ 
DATA MA(2t6>/-'7?/MM2t7)/57,S/ 

DATA ftAt < ii]»/27.?/KAl4«Z)/.2/'lAI4s3J/27«2/MA(4«7i/lC<7/ 

DA TA PBJl»3)/-2»£/PBf3lp2>/-l'2/ 

DATA PA|!tli3l/-.?3/P«<l':«lW-iC6 2 33/ 3 A11i2'2)/'G 7772/PAMi2fll/-.72/ 
DATA PM2f2ill/-pl9C3Z/PAlZ*ZtZW i 2:2H2i'PM2i*fJW.07SeZ/ 

DATA PM ! n<ii'}/1«]/ 

DATA Gf]i3m./Gf2tl)/»?771*/bl?«ZI/.IS2:>32/G13ill/lt/GI<lf*l/l'/ 
CONSTANT TM-.C2CiOa-.OLSiia-2*C 
IF TNA L = ZC . • CLK T IK=I IQC* 

»_3=*31 

SLR AT = .01745323 

T NE T ? - 0 , tPS IZ = C. 

I C 1 J . *IC2 = -Z .5 
A 1 IZL. 7 -A 21-Q. . W 3 I-C • 

Kl=35CG.iK* = 35C'G.iK3=Z0GG.»K*i = 20 00 * 

31 -<C 4 . 5 2 = 20 . t 33-10. • 5<I = 1D. 


CONSTANT 
CONST AN 1 
CONS ? ANT 
CONSTANT 
CONSTANT 
CONSTANT 
CONSTANT 
CONSTANT 
INIT 3 A L 

NH - ^ 

PO=5 7.2S*7?3!- 


$ Dfizl./RO j &M1 CI-ICI^RQ * G M2 C I r IC 2 / RO 


GM1 I - I C I / R 0 l GMZIiICZ/RO 
CALL M6QY TLfNMtHi N6 iKA iPB«F A tOi PI I 


PZI *P A I tPB I ■ => Cl ^ 4CK I IN X I.L *PHI Z* THtTZiPSI 21 

END 

DYNAMIC 

IF (1 IKE » £E . TFINAL > GO TO SI 
DEf7ll/ATIV£ ROOTS 

VARIA9LE 1 I K E - Q . 

CINTCRNAL CI=,C3 

y £ RRCR W3 =1 *E-ethZ = I.E-5 -tt3rl # E-b. GK3=1 .E-1 
ST*CLK CLKTIM 

OUTPUT 5D«w1D*W?Dtw 30»^1 f U ? . W 3 » T H E T 1 t T H E T 2 • T HE T 3 . 

INltIN2iINT»GMliGM2tGM3#GMM*PLl*PL2 
PREPAR WI i W2 f K3»?HE11 # T METZ* THEl 3»GK10»5M20# • *.# 

GM Jf GK2 ■ GK30i GM40« GM 1 # GM4 

NDSOR T _ _ 

MOU'ISMI S SrtO I 2 > - W2 V hi □ T 3 |V« 

PL I - RD • G M 1 5 P L2 “ RD * G h? 

GMT 1 ! - GM 1 $ 3 M < 2 1 - G H 2 % GH J 31 = GM 3 * GMM)=SH4 

GKDUI=GH1D $ tKDI21_-GM2D $ GM D j i 1 - GM 3D t „ GM 0 (_M J r GM40 

COMMENT 

COMMENT CELESTIAl-SENSCR- MEA SURED AN G L E 5 

COMMENT “ 

0 1 * D 2 - H C K I M A 1 R I X t P 2 C * P A G i P S 0 i P CO l 
N X 0 i N Y 0 i N 2 0 = A C : <J II 7 0 9_* G « iCni, *D 1 <021 

lTcV'l ipoYl zorHck.i’i ibeiY. ro\ *o.Voi rozi 

T H E 1 1 - AT. AN 2.1 N Y 0 i N Z 0 1_ . . * .. 1 H E T 2 - A 1 A _N2 I r NX Q j_NZ Q l * . ... - 

THE1 3= AT AN2 ( -L YO i L X C I 


Fig. 15. Case 2 simulation using MBDYTL 
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COMMON! 

COMMENT RATE FEEDS AO .$ ISN A L AND SWITCHING AMP. DYNAMICS 
COMMENT 

INH-THE U 1 INtf = -THE1?« 

AM 3 1 = 5W AMP ( I I ME » T M i Oy ♦ HI) 

A M PZ = S W A M P (I I M E t I M » D 3 r I N7 ) 

• MJ>3=Swa?PHlNEtTM.3D,lN3l 

COMMENT 

COMMENT PLATFORM POSITION COMMA^GS 
COMMENT 

GMT CO-SL°A T*l T • - i T EP i 2 . i T IMc j j 


IN3 = - 1 HE Ti-T R* W3 


GM 1C= INI EG < GH1 CD. J MIC I J 


TORQUE 


T 3 f 3) “ L 3 ♦ AMP 3 


GM2CD~SlPA T + (STEP C E . i T I K £ 1 -STEF <10. » T Iht T CM2 C = INI EG \ GM2C0 >GM2 Cl ) 
COMMENT 

COMMENT REFERENCE BODY EXTERNAL TORQUE 
COMMENT HINGE T03GUE5 
COMMENT 

T 3( ]J =L 1 * AMP1 •$ T 2*AMP2 i T 3 t 5) = L 3 ♦ A M? 3 
TH U | = -K! • TGKl - SMU J - 3J *GM10 
THT 1 1 :-K2*( 5M2- GM 2C 1 - 3 2* CM 20 
TH T i | r- k 3* GM3- R 3* 3K j 0 

COMMENT 

COMMENT SYSTEM ANGULAR ACCELERATIONS 
COMMENT 

CALL MRATETNHi T H t 7 B i I A f F S * F AtGfTi &MDtGHDDtK0»W001 ) 
a I 3 =W DO Till 1i *23 -WOO T \l J * W30 = W00TT3» 

COMMENT 

COMMENT SYSTEM AVGULAT RATES AN u POSITIONS 
COMMENT 

WirlNTEGIWDOH ] ) , Will 
W2-INTEGT WOCT (21 iUZI l 
W3=IN1ERIMD01I 3)fW3I) 

GM10 = IN1E GT WOO Ti^ I tQ . I f 5*20=1 NUGIWQ0M5* 

GM3D=INTEG(U30II5JiC.J 5 5 M<T 0 r I N T E G T WO 0 7 I 7 > 

GMlrlNTEGtGMTDiGMJI) 5 GK2 = INTE(?(GM2 DiSM2I) 

GM3=INTEGtGM30»C.J 5 GM M - IN 1 E G I G M *40 ♦ C « ) 

COMMENT 

COMMENT HCK PARAMETER RATES AND POSITIONS 
COMMENT 

PZOatPAQ9iP3aOiPCaQ = HCK<HCKiPZDi 3 A0*P3Oi?CO>WliW2 
PZ0=INT£6 ( P ZC 0 tPU) 

PA3=INTEGT PAOO* PA U 
P8 0=IMUC IP800 »P8I ) 

PCO~JNTEG( PCOQ.PCI J 

END 
END 
END 

TERMINAL 
SI.. CONTINUE 

END" ’ 

END 


SIMONS 


* 5M2D=InUG (W00M5 T| Q , 
S GMtlQrlNTEG < WQUT f 7 } #C. 
GM2HNTEGI GM2DiSM2I) 

GMM- IN TEGIGMMQtO. ) 


3 AO*P30i?CO»^l iAl »U3I 


Fig. 15 (contd) 
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Fig. 16. Results of Case 2 simulation using MBDYTL 
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Fig. 16 (contd) 
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Fig. 16 (contd) 
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IV. Partially Linearized Systems 

A. The Equations 

One further distinction that can certainly be made in characterizing systems of 
hinge-connected rigid bodies (somewhere between those systems where all hinges 
are permitted unrestricted rotation and those systems whose hinge and reference 
body rotations are all “small”) is the partially linearized system. In this report, the 
term “partially linearized” will refer to the case in which the reference body rota- 
tions are unrestricted and some, but not all, of the y * (k e 0) and their derivatives 
are considered to be “small.” 

The indices of those angles for which linearization is in some sense justified may 
be collected in the set (ft, and, using the expansions of Eq. (9), the linearized form 
of Eq, (8) becomes 

(a 0 o + tfr.o) + 2 3o*Y* + 2 (d 0 h + S 0 k) 'ik — 2 (-E* + £*) (15a) 

urn U® 

(die H- Sio) + 2 Gatin + 2 (Sit + aik) y k 

ug>-(A 

= g iT 2 [C i0 (£* + £*) + C i0 E*] + t< + Ti (15b) 

It should be noted that, for a wide class of systems, it can be recognized in advance 
that e ik = 0 for ie(ft and ke&> — (ft. This is the case whenever none of the hinge 
points whose indices comprise (ft lies on a path from / 0 to any hinge point whose 
index is not in (ft. In physical terms, this means that the small-angle rotations are 
confined to terminal appendages , each of which is attached to only one body whose 
index lies outside of (ft . In fact, primary attention is focused on those scalar ele- 
ments aoc in (15b) where i,kt (ft and i y k are members of the same terminal append- 
age set. It can be shown that such elements a ik are all constants, 

Def. 38. For v e£B, let (ft v denote a “terminal appendage set” Thus (ft v is a subset 
of (ft , containing the body indices of one or more terminal appendages 
which share a common base body, v. 

By appropriate exchanges of rows and columns of the symmetric system matrix, 
the constant 5** s may be isolated in an r X r symmetric matrix, A c , forming the 
lower right-hand corner of the original matrix. One can then employ a matrix parti- 
tioning scheme, where the constant matrix A e is inverted just once, and a time- 
varying matrix equation, now of order (n + 3 — r), is solved at every integration 
step. Clearly, the size of A c should be maximized for the greatest computational 
advantage, and, since only one terminal appendage set (there may be several) may 
be used to form A c , the largest terminal appendage set should be used. 

With this approach, i.e., a single partitioning of the system matrix into constant 
and time-varying parts, another version of the MBDY subroutine, called MBDYPL, 
was written to solve the partially linearized case. However, in a move to further 
simplify and speed up the computational task, Eq. (15) was modified as follows: 

% o*>° 4- 2 = 2 % (16a) 
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(16b) 


where 


C iioU >° + 2 GikYk + 2 GikYk ~ g* T C %0 2 ^ik^k + Ti 

ke$>~& ke&\ k*& 

- - i ^ j_ 

doo = Goo “T floo T- ■ ' ' 

- — , /N . 

0()fc = #0* + duk "T- ’ * ‘ 

A ^ 

dio — flio + flio "h ‘ ■ ' 

^ , A . 

dm = tfi/t + + ‘ * * 


The intent of Eq. (16) is to avoid having to form the separate (quantities a 0 fl , floo, 
Oojt, dfijc, etc., and instead, to compute only the combmed^forms a 0 o, a 0 *, etc., using 
the combined form of the coordinate transformation, C* fc . This approach saves 
overall computation time, although it allows the retention of some second-order 
terms in the linearized variables that would have disappeared otherwise. Thus, 
Eq. (16) is the matrix equation actually solved by MBDYPL, a somewhat more 
accurate representation than that of Eq. (15). 

B. Subroutine MBDYPL Usage 

Just as in the other subroutines, MBDYPL is entered by means of either of two 
call statements, one of which initializes the program with the system constants. 

Initializing Call Statement 

CALL MBDYPL(NH,H,MB,MA,PB,PA,G,PI,LI ? TI) 


The call statement arguments are exactly as defined previously, with exception of 
the two additions 

LI(i) — array of linearized variable indicators; If y* and its derivatives 

are assumed small and therefore are to be linearized, LI(t) = 1. Other- 
wise, LI(t) — 0.) 

TI (i) = array of terminal appendage body indicators; ie£P(If TI(i) = 1, is 
a member of the largest terminal appendage in the system. If TI(i) = 0, 
it is not, although y* could still be a linearized variable.) 


It should be made clear that LI(i) is an array to be supplied by the user; i,e,, it is 
an input array. On the other hand, TI(i) is calculated by the subroutine for the 
user s information, and is an output quantity. As mentioned previously, it is desir- 
able to identify the largest terminal appendage of the system and its member 
bodies. An algorithm to do just that is located in the initializing portion of the sub- 
routine, and the results of its calculations are later used for optimum rearrangement 
and partitioning of the system matrix, as well as made available to the user in TI. 


As before, MBDYPL is executed at every numerical integration step using the 
dynamic call statement. 

Dynamic Call Statement 

CALL MRATE(NH,TH,TB,TA,FB,FA,GM,GMD,GMDD,WO,WDOT) 
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All arguments of the dynamic call statement are exactly as defined for MgDY, with 
the understanding that TH(i), TA(i,/), and FA (ij) refer to T\ and F\ respec- 
tively, when ie&l. In general, any torque or force expressions should be free of 
second-order terms in the linearized variables (for which LI(i) = 1) or their 
derivatives. 

Tlie necessary specification statements for MBDYPL are the same as for MBDY, 
with the addition of the integers LI and TI. 

Required Specificatio7i Statements 

INTEGER NH, H(n), PI(n), LI(n), TI(n) 

REAL MB(7), MA(n,7), PB(n,3), PA(n,n,3), G(n,3), TH(n), 

TB(3), TA(n,3), FB(3), FA(n,3), GM(n), GMD(n), 
GMDD(n), WO (3) 

DOUBLE PRECISION WDOT(n + 3) 

Again, the statement, 

PARAMETER Q = r i 

must appear within the subroutine MBDYPL to properly allocate internal storage. 
A complete listing of MBDYPL is provided in Appendix C. 

C. Application of MBDYPL to the Five-Body System 

1. Large platform maneuver . Since MBDYPL is designed to accommodate both 
large and small rotations in a hinge-connected system, Case 1 would be the obvious 
dynamic maneuver with which to test MBDYPL. 

Recall that the maneuver consists of rotating the platform from its initial position 
(y 3 (0) = 218 deg, y 2 = ~30 deg) to the position y x = 168 deg, y 2 — 20 deg by simul- 
taneous slewing about both axes at the commanded rate of 1 deg/s (starting at 
t = 0). 


The main simulation program listed in Fig. 6 can be used virtually “as is” by 
MBDYPL, with only the addition of LI(4) and TI(4) to the INTEGER statement 
and LI input elements to the DATA statement. In this case, linearization will be 
requested for y s and y 4 so that LI(1) — 0, LI(2) = 0, LI(3) =* 1, and LI(4) = 1. 
Examination of TI, after executing “CALL MBDYPL (NH, . . would show it 
to be identical to LI; i.e,, bodies 4* and would be identified as members of 
the largest system terminal appendage. The modified simulation program using 
MBDYPL is listed in Fig. 17. 

Results of the platform maneuver, Case 1, using the partially linearized approach 
are shown in Fig. 18. Comparison of these to the results produced by MBDY in 
Fig, 7 shows the differences to be indistinguishable. Figure 19 expands the plots 
of to 0 components for just the first 10 s for closer comparison (with Fig. 8). 

While the accuracy of the MBDYPL run in this case was apparently excellent, 
the speed advantage over MBDY was only very slight, i.e., 430 versus 440 s of com- 
putation time for this four-hinge system. Since MBDYPL does perform a number 
of manipulations associated with partitioning and rearranging the system matrix as 
well as matrix multiplication to solve the partitioned system, some tradeoff between 


JPL TECHNICAL REPORT 32-1592 


81 



the time used to accomplish these tasks and the time saved by recognizing certain 
constants and advanced matrix inversion should be expected. Just exactly where 
the “break-even” point between MBDY and MBDYPL speed is depends on both 
the number of hinges and the number of linearized hinges. However, in the case 
simulated here, we seem to be quite near the break-even point. In contrast, the sys- 
tem simulation in Ref. 7 with 14 hinges (10 linearized) was speeded up by a factor 
of two using MBDYPL. 

It should also be noted that the subroutine MBDYPL does accommodate pre- 
scribed motions at any of its hinges, as do the other subroutines, whether the hinge 
is linearized or not. 


V. Summary and Conclusions 

This report confines itself to the consideration of mathematical models useful for 
the description of hinge-connected rigid body system dynamics and, further, the 
computer simulation of those idealizations using general-purpose subroutines. As 
the sequel to Ref. 9, it documents in explicit detail the application of each of these 
subroutines under the circumstances for which they were especially designed, i.e., 
MBDY for unrestricted systems, MBDYTL and MBDYL for totally linearized sys- 
tems, and MBDYPL for partially linearized systems. In each case, the subroutines, 
which are written in Fortran V, serve as the powerful tool that is called upon by 
the main simulation program (written here in CSSL III) to calculate vehicle atti- 
tude dynamics. 

In the process of drawing conclusions about the relative utility of these programs, 
one must certainly begin with the obvious conclusion that MBDY, since it solves 
the exact and unrestricted equations of motion for the system, serves as the ulti- 
mate standard of accuracy for the rest. However, MBDY need not be restricted in 
its use to just the role of a “check routine.” For small systems, it will run faster 
than MBDYPL, and one can certainly envision system rotations large enough to 
make the linearized routines completely unsuitable for the simulation accuracies 
required. Further, the judicious use of the option available in all the routines for 
prescribed hinge rotations can speed the running time of a simulation using MBDY 
remarkably. Thus, MBDY should be used often and to good advantage. 

Of course, the utility of the completely linearized dynamic simulation sub- 
routines, MBDYTL and MBDYL, was well exposed in Section III. For large or 
small systems of bodies in which all the hinge and reference body rotations are 
deemed “small,” either due to fortunate circumstance or deliberately controlled 
constraint, the use of MBDYTL or MBDYL can offer tremendous speed advantages 
over MBDY and still maintain acceptable accuracies of the dynamic response. This 
totally linearized approach has proved to be of the greatest value in analyzing con- 
trol problems at JPL, since most systems fall, or can be forced into, this category. 
In fact, the accuracy of MBDYL has been more than adequate for a large variety 
of space vehicle control problems studied at JPL. Of course, if the vehicle control 
dynamics can also be linearized, the inverted attitude dynamics matrix used by 
MBDYTL or MBDYL can be used directly in control stability analyses, i.e., to com- 
pute overall system root loci and/or eigenvalues. 

Finally, the utility of MBDYPL is perhaps the least of the four routines, although 
it still offers some advantages, particularly for large systems. Where there are a 
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few, very large hinge rotations to be simulated but the large majority of system 
hinges exhibit only small excursions, MBDYPL may offer the only chance to achieve 
the desired accuracies in a reasonable simulation time. In similar circumstances, it 
may serve as a more economical reference or check solution source than MBDY. 
All in all, the partitioning scheme employed in MBDYPL, while not offering the 
tremendous speed advantages of MBDYL, at least provides some gain in efficiency 
in certain specialized instances. 

As for the general-purpose character of all four subroutines, it is felt that their 
presentation in Fortran will make them widely useful, both to those who are com- 
fortable with special-purpose simulation languages and to those who prefer to 
write their own simulation programs and integration subroutines. In order to make 
the routines more easily accessible, the entire package has been submitted to 
COSMIC (Computer Software Management and Information Center), University 
of Georgia, Athens, GA 30601, for evaluation and dissemination to interested gov- 
ernmental agencies as well as industrial, educational, and research institutions. 
While many may wish to further specialize one or more of these programs to their 
own particular needs, the four programs, as they are described here, can serve as 
a reliable and valuable addition to any simulation subroutine library. 
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PROGRAM 5-BODY SYSTEM SIMULATION USING MBDYPL CASE 1 

INTEGER NH*PIf 4 l»H<4 >.LH4I.TIf4l 

ARRAY MB<7J.MA(4.7I.P8l4f3>.PA<4r4*3).Gf4»3».TB<3>»TAt4»3> 
ARRAY FBf3).FA«4»3)rTH(4>*GM C4I .GKO <41 *GMDD I 4) *WO 13 > 

DOUBLE PRECISION W00T(7I 

data H/o»i»o» 3 /Ri/o*o»n»c/Li/n»u»i *1/ 

DATA M8/U5.»31G. »440.»14.*-14.»34.6.410./ 

Data KA<lrll/.35/KAf l»2»/.35/MA (1.71/6.8/ 

data M A (2.17/4. 95/MA<2.2)/2.2/MA<2.3l/5.5/MA(2.4)/-.41/MA(2.5l/.G7/ 
DATA MA(2.t»/-.54/MA(2»7)/57.5/ 

DATA MA(4»l)/27.2/MA(4r2l/.2/MA(4»37/27.2/MA(4*77/10«7/ 

Data FBt 1 .3I/-1-5/PE f 3.27/-1.2/ 

DATA P A( 1»1«3>/-. TS/PAt 1.2,2J/-.lC/PAU»2.3)/-.75/ 

Data FA(2.2»27/-.22/PAf2»2.37/.20/ 

DATA PAI4. 4.21/3. 3/ 

Data Gtl»31/i •/Gf2.1>/-1./G(3*ll /l. /G 14.31/1./ 

CONSTANT TM-.02D.03-.005.TR-2.0 
CONSTANT TFINAL:6Cl..CLKTIHrl 100. 

CONSTANT L1=.23.L2-.21»L3=.31 
CONSTANT SLRAT=.D174532B 
CONSTANT PHIZ-O. .THETZ-G. .PSIZ=0. 

CONSTANT H1I-0. .W2I-E' • »W 31 -f » 

CONSTANT K1=3500.*K2=3SOO..K3=2000.»K4=2000. 

CONSTANT 81-20. »B2s2C**33 = 10.»64rl0 . 

CONSTANT TC1-218..TC2--3Q. 

INITIAL 

NH=4 

RD=57. 2957795 * OR-l./RO S GKICI^IC 1/RD * G«2CI=IC2/RO 
GH1I=IC1/R0 t GM2I-IC2 /RD 

CALL M9DYPL fNH.H.PGfPA.PBtPA .G.PT *LI*TI> 

PZI.P AI»PBI»PCI=HCM INTTL * PHI Z » THETZ .PSTZ1 

EMC 

DYNAMIC 

IFf TIME.GE.TFINALJ GO TO Si 
DERIVATIVE B0DY5 

VARIABLE TIKErO. 

CINTERVAL CTr.OS 

XERRCR Wl-l.E-6»W2=l.E-6»W3-l.E-G*GM3=I.E-4 
STPCLK CLKTIM 

CUTPUT 50»WlC«N2D*H30tUl.U2.H3»THETl»THET2»T HET 3 » . . . 

IN1.IN2.INI.6h 1.GM2.GM3.GW4.PL1.PLZ 
PREPAR UlfH2»W3tTHETl.THET2»THET3,GHlD»GH2D»... 
GM1.GM2.GM3C.GM4C.GM3.GH4 

NO TCP T 

WO 1 1 I =W I * W0I2I-W2 t WO ( 3 7 - W3 
PLI=RD*GM1 * F12=RD«GH2 

GHfl»=GMl * GH|21=GM2 S GM C 3 >-GM 3 « GM(47=GK4 

OHO <11 =GW ID * GHD(2I=GM2D * GmOIJI=GM30 $ GM0(4 7=GM40 

COMMENT 

COMMENT CELESTIAL -SENS OR- MEASURED ANGLES 

COMMENT 

Ol»D2=HCK(M#TRIX.FZ0.PAC.PBQ.PCO7 
NXO.NYO. NZO-HCK< IT0B.0..0..1. .01.027 
LXO .LYO .LZOrHCK (IT0B» l.»C.*C!. .01.02 » 

TH£Tl=ATAN2t NY0.NZO7 * THET2=ATAN2t-NXO.NZO» 

THET3-ATAN2 C-LY0.LX07 


Fig. 17. Case 1 simulation program using MBDYPL 
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RATE FEEDBACK SIGNAL AND SWITCHING AMP. DYNAMICS 


S IN3=-THET3-TR*W3 


GNIC^INT EECGMlCD.GHlCI I 
GN2C=INTEG( GM2CO.GM2CI) 


TB ( 3 f"L3* AMP3 


COMMENT 
COMMENT 
COMMENT 

IN lr-THET 1-TR *W 1 $ IN2 =-THET 2-TR« W2 

A HPlrSW AMP ( TIME.TM.OB.INI > 

AMP2rSW AMP (TIME .TM.DBfIN2) 

AMP 3rS VAMP (TIME *TM.D9»IN3I 
COMMENT 

COMMENT PLATFORM POSITION COMMANDS 
COMMENT 

GM1C0-SLRAT»( “1 •'•STEF ISD.EI.TIME) ) * 
GH2CO=SLRAT*(l.-STEP<5tN*TIHEl J S 
COMMENT 

COMMENT REFERENCE BODY EXTERNAL TORQUE 
COMMENT HINGE TORQUES 
COMMENT 

TB( 1>=L1*AMF1 *. T8(2)=L2»AMPZ * 

THU >=-Kl*<GMl-GMlC)-8l*GHlD 
TH( 2»r-K2* <G M2-GM2C > -B2*GM2D 
TH(3)=-K3*GM3-33*GM30 
TH(m=-KM*GHA-B 1 l*GM | *0 
COM MENT 

COMMENT SYSTEM ANGULAR ACCELERATIONS 
COMMENT 

CALL MR ATE (NH t TH # TB . T A » FB . FA . GM »GMD tGNDD * WO . WDOT I 
WlDrWDOT(l) i W2D=WDOT(2) $ W3D=WOOT(3> 

COMMENT 

COMMENT SYSTEM ANGULAR RATES AND POSITIONS 
COMMENT 

W 1-1 NTEG ( WD OT( 1 ) » WIT ) 

W2=INTEG(WDOTI2».W2I> 

W3-INTEG(W00T(31 »W3I» 

GMlD-INTEG(WOQT(«I.O.> * 

GM30=INT£G(WD0T(6I.O. J * 

GMlrlNTCG (GVlOfGMlU * GM2 
GM3-INT£G(GM3D»0.I S GMT 
COMMENT 

COMMENT HCK PARAMETER RATES AND POSITIONS 
COMMENT 

P ZOD . P AOC »P80D.PC00-HCK( HCKfPZ0*PA0»PB0»PC0»Wl»W2»W3S 
PZOrlNTEG (PZOD.PZI» . 

PAO=INTEG(PAOD»PAI> 

PBOrINTEG (PBODf FBI I 
PCO=INTEG(PCPD.PCI1 

ENC 

END 

ENO 

TERMINAL 

SI.. CONTINUE 

END 

ENG 


GMZD=INTEG(WD0TC5>»P.l 
GHAO=INTEG( WOO Tf 71*0. » 
:INTEG(GM20»GM2I) 
:TNTEG(GMAD»0. J 


Fig. 17 (contd) 
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Fig. 18. Results of Case 1 simulation using MBDYPL 
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Fig. 18 (contd) 
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Fig. 18 (contd) 
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Fig. 18 (condt) 
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Fig. 18 (contd) 
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Fig. 18 (contd) 
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Fig. 18 (contd) 
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Fig. 18 (contd) 
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Fig. 18 (contd) 
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Fig. 19. Results of Case 1 simulation using MBDYPL (components of first 10 s) 
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Appendix A 

Subroutine MBDY Description 


I. Algorithm for Calculating Direction Cosine Matrices, C 0r (r«<0) 

As was pointed out in Section IIC, all the coordinate transformation, or direction 
cosine, matrices, C ri (r, j € £3) need not be calculated if all the system equations are 
expressed in the reference body (/ 0 ) coordinate frame. Thus, the objective is simply 
to obtain C 0r for all re^. An algorithm for accomplishing this is incorporated in 
MBDY and in MBDYTL and MBDYPL as well (MBDYL uses no direction cosine 
transformations). The computation procedure is as follows: 

Step 1. Let r = 1 

Step 2. Calculate elements of the direction cosine matrix C ir 9 where / = h r (see 

Eq. 6) 

Step 3, If / — 0, jump to Step 5 

Step 4. Calculate elements of C 0r from relation C 0r — C 0j 'C /r 

Step 5. Let r = r + 1 

Step 6, If r < u + 1, go back to Step L Otherwise, stop. 

The calculations involved in Step 2, of course, may include the use of sin y T and 
cos y r , or they may use approximations thereof (i.e., y r = sin y r , cos y r = 1), depend- 
ing upon whether linearization is being employed or not. This sort of bootstrapping 
calculation of C or , by way of Step 4, is made possible by the fact that / is always 
less than r, and therefore C 0; and C jr must necessarily have been computed before 
coming to C 0r . 


II. Summary of MBDY User Requirements 

Subroutine Entry Statements 

CALL MBDY(NH,H,MB,MA,PB,PA,G,PI) 

CALL MRATE(NH,TH,TB > TA,FB J FA,GM,GMD,GMDD,WO,WDOT) 

Input /Output Variable Type and Storage Specifications— Fortran V 
INTEGER NH, H(«), PI(n) 

REAL MB(7), MA(n,7), PB(n,3) f PA(n,n,3), G(n,3), TH(»), 

TB(3), TA(n,3), FB(3), FA(n,3), GM(n), GMD(n), 
GMDD(n), WO(3) 

DOUBLE PRECISION WDOT(n + 3)(where n = number of sys- 
tem hinges) 

External Subroutines Called 

SOD— double precision matrix equation solving subroutine; user-supplied (see 
line 437 in MBDY listing) 
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This subroutine solves the matrix equation Ax = EC, where A is of order 
IV X IV and IV ^ 3. The solution is returned in EC. 

Subroutine Setup 

Insert the Fortran V statement (see listing) 

PARAMETER g=n 
where n = number of system hinges. 

Data Restrictions 
n^l 

Core Storage Required 
Code: 2000 words 

Data: 500 words (minimum; increases with n) 

Listing 
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SUBROUTINE M 0OU N H t H • M3 * MA * Pfi * P A • G i P II 


1 * 
2 * 
3* 
4* 
5* 
6* 
7* 
8 * 
9* 

2 0 * 
11* 
12 * 
13* 
14* 
15* 
16* 
17* 
18* 
19* 
20 * 
21 * 
22 * 
23* 
24* 
25* 
26* 
27* 
28* 
29* 
30* 
31* 
32* 
3 3* 
34* 
35* 
36* 
37* 
38* 
3 9 * 
40* 
41* 
42* 
43* 
44 * 
45* 
46* 
47* 
48* 
49* 
50* 
51 * 
52* 
53* 
54* 
55* 
56* 
57* 
5 0* 
5 9* 
60* 
El* 
62* 
63* 
64 * 
G 5 * 


C 

C AOJUSTA0LE DIMENSIONS 

C 

INTEGER HI 1 } iPIl 1) 

REAL MBm»MINH.71*PBtNHi3TiPMNNiNH*3|fG!NKi3) 

PARAMETER 0=4 

PARAMETER S=a*lfV=G*3iV4r4*\liS3 = 3*S 
C 

C ACCI1ICNAL DIMENSIONED VARIABLES 

C 

0CU6LE PRECISION A ( V » V 1 * W R K ( V 4 ) • B M A SS I S } 

DIMENSION PHtS»3fJliC)HS'SJ*DYtSiS)tfD2tS9SlfDX0IS«SI»0V01StSJ«DZO4 
$StS>»i.XIS*S>«L1tS.SWLZtS?S!tFLXO!SUfE¥OlSwFEZOlS!*60lGi3HTXGlS 

t)*I10lSJ*TZQTS) • P S t S ♦ S # 3il) t A S I Q * Q ) iAVTQi3I»T 1 1 « S * 3 * 3 ) t A G 0 T 3 * 31# AB 
$<2.3)fAISI3>*EAm*Cwm*CE(3> 

INTEGER HKI a I *EP S I G » S ) 

REAL LX.LY*IZfIXX < S 1 * I Y M $ I i IZ 2 t S I »IXY I S 1 1 1 X 2 m * I Y 2 I S ) 

EQUIVALENCE I A i P ST *ILX*DXO*ASI • ( t Y # 0 Y 0 ) «ILZ*D201 
N F = AH * 1 
C 

C IMTIALIZE EPS IK* J I WITH ZEROS AND DEFINE KK l« 1 

C 

DC 85 K = 1 * N H 
KK I K i = K * 1 
OC 85 J=1*KB 
85 EPS<K*J) = G 

C 

C DEFINE EFSlKaJ) USING HtJ) 

C 

DO 06 KS1 *NH 
Df 86 J=Z*N6 

IF I K «EQ« I J- 2 I I EPS IK #J1 =1 
IF(K.LT.<J-1M GO 10 87 
GO TO 9 6 
87 CCMINUE 

JO = K * 1 

DO 89 L = J 0 » J 1 
I F IK. GT - IL - U t GO TO 8 9 

IF t ( EPS IK .L I . EQ • 1 I- A ND • I HI J- 1 ) . £ G« IL- 1 U 1 EPS IK# JJ -l 
89 . CCMINUE 

BG CCMINUE 

C 

c INITIAL CALCULATION OF QARmNUR VECTORS W.R.I. BCOY C .G . £ 

C AND HINGE POINTS 

C 

IX XI 

imn^ei2i 
izzi n=«Bm 

IH(1 UKEt 4 I 
IX ZC U = MB15 I 
112 11 T=M0I6 > 

0HASSI 1 I = M B < 7) 

T F = BM ASS 1 1 1 
00 35 J = 2 *N B 
I >X I J) =M At J-l • IS 
II 11 J ) = M A I J- 1*2) 

122 

IX Y< J)=MAlJ-li4) 

IXZIJ!=MM J-lt 51 
II Z< J I =MA i J- 1*6 ) 

BMSStJUKAI J-1'7 I 
35 I H “ T H ♦ 8MASS1 J I 

OC 149 1=1 *N8 
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66* 


67* 


68* 


69* 


70* 


71* 


72* 


73* 

70 

74 * 


75* 


76* 


17* 

400 

73* 


79* 


33* 

600 

61 * 


32* 

500 

83* 


34 * 


05* 


86 * 

90 

87* 


39* 

90 

89* 


90 * 

1C1 

91* 


92* 


93* 


94 * 

163 

95* 


96 • 


97* 

1 4 9 

98* 


39* 


100* 


2 01* 


102* 


103* 


104* 


2 05 * 


106* 

13 

1G7* 

C 

loe* 

C 

109* 

C 

110* 


111 * 


112* 


113* 


114* 


115* 


116* 


117* 


110* 


1 1 9* 


1 20 * 


121* 


122* 


123* 

30 

12 4* 


125* 


126* 

31 

127* 


1 28 * 


129* 


130* 


131* 


132* 

C 

133* 

c 

1 34 * 

c 

135* 


136* 



11 = 1-1 

DC 149 J=1 «NB 
J1 = J-1 

If II .EG- J) GO IG 16 3 
IFII.GI.J) GO 10 7 C 
I F 1 1 ■ E C . ] > GO ID 8 D 
IHEPSm GO TO 4 00 

l y i i ♦ j j = p a t n * ii* i » 

LV4ZtJI=PAIIl«I1«2l 
L i IT * J l = P A ♦ II * II* 3 l 
GO TO 14 9 
C (MIME 
00 600 K- I * J 1 

J F i EPS IK* J ) .EQ *21 GO To 500 
CONI INUE 
GC TO 149 

L X 1 I * J) = P Al IlvKf 1) 

L Y I 1 1 JI=FA < II * KiZ I 
LI < I * JUPM II *K • 3) 

GC TO 149 
DO 90 L - 1 * J 1 

IFtEPS t L * J I .EG cl) GO 10 101 

CONT INUE 

GC TC 149 

L X < IiJ) = P84L t 2 2 

LYII» J 1 = P© I L t 2 1 

lz<u Ji = psa * ii 

GC TC 143 
1X4 I|J|:Q. 

L V t It JUO. 

LZ( I t J)=0. 

C C M INUE 

00 13 N = 1 *NB 
DC 13 J=1 i NB 
DXt Ni J1=LX INi Jl 
Dy < N|J):L HNf J) 

Q2CKiJ)=LZtN. Jl 
DO 13 K - 1 *N 0 

DMAfJUDXlhtJI-IBHASSIKI/TKI*LXIN*KI 

DflN#JirD1MNfJT-IBHASSIKI/TMl*l»fN#Kl 

D2l*»Jl::DZt*tJ>-tBMASSIK)/TM)*LZIN»K) 

CALCULATION OF AUGMENTED INERTIA DVADICS FOR EACH EoDV 

DC 31 N - 1 r N B 
PHI fit 1 1 1 1 - IX X IN ) 

PMU*li2U-IXYINl 
PHINi 1#3 I--IXZIN ) 

PHtN#2f2)=ITYtN) 

PH i N*?0 3U-IV2IN I 
PHtf t3»31=IZZ t N I 
DO 30 J ' 1 i N 9 

PMf till I = FH I N 1 1 1 1) *6M A SS I J ) * IDT lNt«J|**2*0Z I N t J | * *2 I 
PH < N * J # 2) =PH IN i 1 i 2 0 HA S S I J J ♦ C X I N 0 J) * 0 Y t N f J ! 

PFU tl »3l-FHIN0l» 3)**BMASStU)*DXIN» U 1 * 0 i IK* J 1 
PH 4 Ni2. 21 =PHlN02 t 2M6MASSlJ|*<DXIN0j)**2tDZ I NfJ)**2l 
P M N 1 2 i 3 I r P H t N # 2 1 31-BMASS I J ) * 0 ¥ t N t UltDZINtJ) 
PHINt1f3)::PHlNt3.3MBHASSIJI*IDXINtJI**2*0YlNtJT**2l 
PH|N>2»1 )-PH(N«lv2) 

PH < N * 3 # 1 I "P H I N 0 1 * 3 ) 

PHTf*3.2 UPHIf)«2>3 ) 

RE TURN 

EHRY KRATUNHilHtlBfTAiFBtFAiGHfGMOtGNODtWCtECI 

01 MENS ION Y0 t 3) • 1A IN Hi 31 #f 0 Ul • FAINHt3) ■ GMI1I tGMDU It GMODUIt 
ITMAHItUO I 3 k » ti XC t S 1 aWYOISltfeiZCi I S I »E 4 S 3 » 1 1 

0009LE PRECISION ECI 1/ ) 

90DY-T0-B ODY COORDINATE TRANSFORMATION MATRICES 

DO 135 J=1*NH 
K -H l J ) 


102 


JPL TECHNICAL REPORT 32-1592 



137* 


36 M - S IN t GM( J H 

138* 


CGM=COStGM< Ji ) 

i jg* 


CG Ml = 1 • ** C GM 

mo* 


Gl=CGNi*Gt JpI > 

m* 


G2 = CGH1 *Gi J * 2 ) 

142* 


G3 = CG*1 *GUM> 

14 3* 


SG 1=SGM*G IJ t 1 1 

144* 


SG2=SGK*Gi M2T 

14 5* 


SG 3=SGK *GU i 3 1 

me* 


6,1 S - G 1 *Gt Jil ) 

m?* 


G2S=GZ*GtJ*2 ) 

ms* 


G3S=G3*G(U»3) 

mg* 


G12-G1*GIJ#2) 

150* 


G 13=G1«GI J*31 

151* 


62 3=G2*G1J«3 1 

152* 


AEUtl T=CGM*G1S 

153* 


A3U«2) = SG3«G12 

154* 


AEU *3J="S62*G13 

155* 


A3<2.1 1-- SG3*G12 

1 56 * 


A61?i2 1-CGM*G25 

157* 


A £ t 2 »3 1 "SGI *G2 3 

158* 


A3 t 3 • 1 1-562*613 

15 9* 


AEU>2 T=-SGI*G23 

150* 


A3i 3 f 3)=CGM* G IS 

161* 


IF1K*EG*0) 6C 70 3350 

1S2* 


00 3 34 L = 1 • 3 

16 3* 


DC 334 M = l>3 

1 64 * 


Tl 

165* 


DC 334 1=1*3 

166* 

334 


16 7* 


GC TC 335 

169* 

3 35 Q 

CONI INUE 

169* 


DC 335 1 L = 1 * 3 

170* 


□ 0 3 35 1 M - 1 • 3 

171* 

3 3 51 


172* 

335 

CONI INUE 

173* 

C 


1 ?4* 

c 

COORO. TRANSFORMATION OF G VECTORS no REF. 9CDY FRAME) 

1 75* 

c 


1 7 6* 


DO 362 1= 1 # N H 

177* 


DC 362 J = 1 * 3 

178* 


GO 1 I « J ) = Q i 

119* 


DC 362 K = 1 t 3 

1 80* 


GOt IiJJ=GQil*J)*T! l#I*lvNaJ)*G(I*KT 

1 81 * 

362 

CCMINUE 

192* 

C 


183* 

C 

A (• G • VELOCITY COMPONENTS OF EACH 0ODY tIN REF* BODY FRAME 1 

1 84 * 

C 


185* 


DC 361 J = 1 * SB 

186* 


UXOI J 1 = WO 111 

16 7* 


U KiJlrWOl 2 1 

188* 


wzot j i = wam 

189* 


DC 36 K=1*NH 

130* 


IF 1 EPS tK » J J . EQ. 0 1 GO TO 36 

191 * 


WXC 1 Jl =WXC (Li * GMO tK 1*G0 IK *1 J 

192* 


WV 01 J 1 = W YQt J MGMCIK)*GOIKf 2) 

1 9 3 * 


W 2C ij 1 = WZO UT ♦ GMO tK 1* Got K i 3 ) 

1 94* 

36 

CONT INUE 

19 5* 

361 

CCMINUE 

1 96* 

C 


197* 

c 

TRANSFORM AUGMENTED BOCY INERTIA OYAQICS TO REF* BCD Y FRAME 

1 9 8* 

c 


199* 


OC 36 3 1 = 2 f NB 

200* 


DO 364 J= 1 1 3 

20 1 * 


OC 364 K = 1 * 3 

202* 


ABC J «K ) = 0 * 

20 3* 


DC 364 L =1 • 3 

2 Q4 • 


A3!J»K1 = A0tJ,KJ*PHU*J*LI*TU*I*l*K) 

205* 

364 

CCtS TINGE 

206* 


00 365 J= 1 * 3 

20 7* 


OC 365 K = 1 » 3 

2 Q 8 • 


PS 1 I*IiJiKISfl« 
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ZG 9 
210 
211 
212 
213 
2m 

215 

216 
217 
21 8 
215 
220 
221 
222 
223 
22 * 

225 

226 
2 2 7 
228 

229 

230 
2 31 

232 

233 
23* 

235 

236 

237 
236 
239 
2*0 
2*1 
2*2 
2*3 
2* * 
2*5 
2*6 
2*7 
2*8 
2*9 

250 

251 

252 
252 
25* 

255 

256 
25? 
268 
259 
280 
261 
262 
263 
26* 

265 

266 
26 7 
268 
269 
210 

271 

272 

273 
2 7* 

275 

276 

277 

278 

279 


OC 365 L- 1 * 3 

365 CIMINUE 
363 CONTINUE 

F EXC U 1 = F8 (If 
F EKU UFB121 
FEZ0m=F9l3l 
T XC (1 1=181 1 ) 

TYOI U = TB<2) 

T2CI1)=TBI 31 
DO 11 1-2 tN 8 
K :l-l 

FE XO < 1 1 =T U p Ifli 1! *f AIK« 1! M I 1» If 2«1 J«FAIK«2HHl'I'3fl1»F*1Xi3l 
FEYCtIl::TtltI»t*2 1*FMKtllfTUfIf2>2nFMKi2lfTll«Ii3f2!*FMKf3| 
FEZOauMii M< 3|*FAU, II MUiIi2i]l*FMKt2Mm«I t 3il!*flHK,3l 
nMl) =T I 1 » I • 1 r 1 ) * T A I K t 1 )«T|l*I»2rl I»tAlK»2|*T J ♦ T A 4 K t 3 | 

T Y04 IT = IU. Iil.2|*MIH»lMTU»I»2i2IMMKi2l»TlMt3.2l*TMK,3l 
t 4 C 4 1 1 = ll<TniI»2t3|4H|K»ZMlUiIt3»3 UTMK»3| 

00 17 J=1 «N3 

DXCU»J}:T IhlihlMOX lit Jl tUltliM umii J| 

OYQI ItJ 1 = Til • I» 1 t2M0X 1 1. Jl *T U i I t2i 2 UOVI It JT*T I 1. It Ji 2 I *0Z t Ti J> 
D2CIIfJ>=T|1tIiI»3l«DXIIfJ>«m»If2«3MCTII«JI«tn«It3aI|«OZlI»JI 
1? CONTINUE 

OC 36 7 1 = 1 . NB 

OX 0 < M):CXI ltll 
OKU tI)=DY lit I) 

367 OZOI 1# II=D2( l.IJ 
C 

C AO 0 1 1 ION At AUGMENTED INERTIA 0YADIC3 UN REF ,900 7 FRAMEI 

C 

00 3 7 1 = 1 iNB 
OC 37 J = 1 . NB 
IF I I « E Q « J T GO TO 37 

P S I IvJ p 2 9ll = -TH« IDV C lit J) 'D YQI dt-U «DZOlI>.f I «D20t J»I I I 
PSI I f J f 1 » 2)= TH *0 >0 IJ»II*DYO(I*JJ 
PSt IfJ.lt3J=TM«0X0tJ*I)«DZ0iItJ> 

PStIfJf2tl>=TM*OY0 4JiI>*DXOUtJT 

P'tI»J*2t2>:->TH*fDXOfI»J»*OXOIJ’I>«DZOfI»j|«CZOt>JiI>) 
PS(IfJ.2#3J=TM*DYOTJ.n*OZOTI.JI 
PSUvJv3tll:lH*DZQ|J«I»«DX0tX»JT 
PSf IpJp 3.27= TM*0 20 IJ#II*OYOIItJT 

P5II»Jf3t3K~-»TKMDX0(I>J)*DX0IJ»IT *OYOtIffu)*OYOIJ»l ) 1 
37 CONTINUE 

OC 751 H = 1 i 3 
DO 751 N= 1 i 3 

75 1 P ! II tl pMpN 1 =PH 4 1 » M i N i 
C 

c VECTOR CROSS PRODUCTS DESCRIBING SYSTEM ROTATIONAL COUPLING* 

C (QUADRATIC TERMS INVOLVING THE CONNECTING BODY ANGULAR 

C VELOCITIES AND THE MUTUAL 8 A RY C E N T£ R - H I N 6 E VECTORS) 

C 

OC 230 N = 1 i NB 
CP X= 0. 

CFY iQ. 

CPZ=0. 

DC 2301 L = 1 . N B 

IF I N « EG «L ) GO 10 2 3 01 

WCX=W70lLM0Z0 4LiN)-WZ0lL MDYO IL*N) 

WD t = W 2 0 T L ) . D X G 4 L • N)-WX01L)*DZ0U.NT 
WCZ=WXC4LI<DY0 4LfNI“WtOlL I + D XO IL * N ) 

MW DX = TM* I WYO IL I • WDZ- UZO I LI *WDY> ♦FEXOtLT 
WWD1-TM* (UZCtL I *W DX - U X 0 I L )*WDZ1*FEY0U 1 
U A 02 = IF* 4UXCIL >*WOt -UVOlL >«WOX l*FE20tL ) 

D«WDX = D¥OIN tl 7*W WDZ-OZOIN. D* MUOY 
DmoY = DZC4NiL»«VWDX-DX04NfL HttWOZ 
DWWOZ=DXO IN.L ) * W UD Y- D Y 0 I Nt L M WUOX 
CFX=CPX*OMWDX 
CP Y= C P Y ♦ 0 W WO Y 
C f Z =C P Z ♦ C W k OZ 
2301 CONTINUE 

OFTUDVC INi N MFEZO IN J-OZOINtNUFEY OIN) 
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280 * 


Of Y = 0 2 0 < N * N 1 *FEXCIN)-DX<HNfNJ * F E 2 0 1 N J 

281* 


DFZ=DXGINtKI*FEVO IN )'-D V 01 Nt N> * F E X 0 1 N 1 

2 02 * 


HX=PS<NfNil»l)*W*OlNI*PSlN*M#l«2l*WY0m*PSlNiNflf31*WZO1N) 

283* 


Ht = FS|NtNt2»l>*ltX0m*PSlN»N*2i2>*UY0lKHPSlft*h«2«3l«tf20tNI 

2 8 ** 


H2=PStN*N #3. 1)*WX0 IN) *P S tNf N. 3. 21 ♦ WYOI N1 *PS 1 NiN. 3t 3 ) *WZ0 tN) 

265* 


K r 3 • { N - 1 1 

2 86 * 


El K* 1 «1 l = UZO 1NJ-HZ«U VOm MX04NI *CPX«0FX 

2B7* 


EIM2.1>=H2*yXC|NI-HX*WZOtN>*TVOlNMCPY*DFY 

2 8 8 * 


El K* 3« 1 1 = HX • UTO IN l«HV*yXOI Nl • T20I Nl *CPZ * OF Z 

289* 

230 

CCIWINUE 

2 90* 

C 


291* 

* 

** 

ACG MATRIX ELEMENT COMPUTATION 13X31 

2 92* 

c 


293* 


OC 3Q01 in. 3 

2 9** 


00 1001 J = l*3 

295* 

30 01 

acc a* j>=o. 

2 96 * 


00 3 1=1 9 N3 

297* 


0 ( 3 J-l *NB 

298* 


AOOt 1 <1 ) = AQQ Ili 1 l*PSU* J *1 *11 

299* 


ACOUt2l=AOGIl'2HPSIX«J«l»2J 

3 00 * 


AODi li3> = AQ0llf 3 MPSII* Jil *3) 

301* 


AC0C2i21=A00l2i2>tPSIIpJ*2f2l 

3 0 2* 


AOOi 2 # 3 1 = AOOI2t 3 MPSI I# Ji 2 « 3> 

303* 


ACQI3t3) = AGGI3t3MF3(I*J«3»3) 

30** 

3 

CONT INUE 

305* 


ACO 1 2 » 1 ) = A G C 1 1 *21 

306* 


AOOI 3.1 l = A00U»3 1 

307* 


ACO 13«2>=ACG1Z *31 

300* 

C 


309* 

c 

ACM VECTOR ELEMENT COMPUTATION 13X1) 

310* 

c 


311* 


DC 7 K-l iNH 

312* 


Atf(Ktl)=Q« 

313* 


H(K» 2 l: 0 , 

31«* 


AV(K«1)=G« 

315* 


OC 7 I=1#NB 

316* 


00 7 J-l f NO 

317* 


ZFIEPSIKtJI .EQ .0) GO To 7 

318* 


DO 10 K-l f 3 

31S* 


DC 10 L = 1.3 

320* 

10 

AVIKfHI^AVIK«H3*PSlIfJfN*Ll«60IKfLI 

321 * 

7 

CCMINUE 

322* 

c 


323* 

c 

A F M SCALAR ELEMENT COMPUTATION 

328* 

c 


325* 


OC 1* tUl'NH 

326* 


DO 1* Mr 1 iNH 

327* 


IMK.GKM GO TO 1* 

328* 


A I St lira. 

329* 


A I S <2 1 ~0 • 

3 30 * 


AISI 3UQ. 

331* 


DC 15 J = 1 .KB 

332* 


DO 15 IrltNB 

333* 


Iff IEPS IKt J) .EG .0 1 .OR . 1 EPS IM»I1 .£Q .OM GO To 15 

33** 


(X) 10 Nr 1 .3 

335* 


OC 18 1-1*3 

336* 

18 

Al SI N 1 - A I S 1 N ) *P S 1 J# I • N«L)*G0IH»L) 

337* 

15 

ccminue 

330* 


A5|K«HI-G0lK » 1 1 * AISI 1) * GO ( K . 2 > ♦ AISI 2T*GOIK»3)*A3SI3l 

339* 

1* 

C CM T INUE 

3*0* 

C 


3*1* 

C 

CCMFU7E CORRECTION ELEMENTS FOR 1 E 1 VECTOR 

3*2* 

C 


3*3* 


DC *0 I=1*NB 

3 ** * 


EA i 1 1 = 0 . 

3*5* 


E A 12 1 =0 • 

3*6* 


EA( 3 1 ” 0 • 

3*7* 


0 C * 1 J = 1 1 N B 

3*0* 


DO *11 M=l*3 

3*9* 

*1 1 

Ck t Ml = 0 * 

3 5 G * 


DO *2 K-l *NH 
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351 • 


I F C EP S (Kt J I.EQ.OI SO TO 42 

352* 


KB -K K < K \ 

353* 


Cfc m - CU m *GMD CK »' WV 0<KB 1 *60 (K» 3 t - UZCIKB t ‘GQIKf 2 ! 1 

354* 


CWI2l=CiM2l«fiMDIKI*U(ZOH<aJ*GOIKilJ-W)(OIK8»*GOIK#3U 

355* 


CM 31 = CW 13 > «GMD IK M IWXQlKBMGO <K*2MWUlK8)*G0 1K.l II 

3 5 6 * 

42 

CONT INU 6 

35 7 * 


DC 45 K = lt3 

3 5 0 * 


00 45 L = 1 *3 

359* 

45 

E M M - E A 1 M ) *PSjItJfM*L ) * C W ( L 1 

350* 

41 

CONT XNUE 

361* 


k 1 = 3 * ii-n 

352* 


El K1+1»1I-£IK1*3 tl!-£AllJ 

363* 


6 IK1*2.1)=E 1K1 *Z% 1 ) -E A 1 2 1 

364 * 


E\ Kl* 3* 1} =£1 K 1* 3.1 l-EM 3) 

365* 

40 

CCMHUE 

3 66* 


00 5 5 HI- 1 .3 

367* 

55 

EClPXUEfHl'll 

3 6 0 * 


DO 52 J - 2 .N B 

36 9 * 


DC 52 H - 1 t 3 

3 70 * 


Kl=3*«J-l MM 

371 * 

52 

E CtM = ECtM>*EtKI.n 

3 72 * 


I- Q 

3 73* 


DC 60 K = l.lNh 

3 74 # 


IFtPIlK I.NE.Qt GO TC 60 

3 75 * 


I :M 

3 76 * 


EC t I * 3 J = 0 ■ 

377* 


DC 601 M = 1 . 3 

3 7 0* 

SOI 

CE(M 1=0* 

319* 


DC 61 NB 

3 90 * 


IF(EPSlK.JJ*EQ*OJ GO TO 61 

301 ♦ 


DC 65 MSI s 3 

3 82* 


J1 -3*J J-l J * M 

383* 

65 

CEfM = CE 1 M H£t Jit Jl 

384* 

61 

CONT INUE 

3 3 5* 


00 6 6 L - 1 1 3 

366* 

86 

E t< 1*3 J=EC 1 1*3 1 *60 IKiL l*CE 1 L > 

3 87 * 


EC i X + 3 1 = ECl I ♦ 3) « IHIK ] 

308* 

60 

CCMINUE 

3 8 9* 


DO 810 1=1.3 

390* 


DC 610 J=1 .NH 

391* 


IF { P 1 1 J ) » EG « 0 ) 60 TO 610 

392* 


ECm = ECIl)-Atf IJt IMGMDDf J) 

393* 

610 

CONT INUE 

39 4* 


K -0 

395* 


DO 6 12 I=ltNh 

396* 


IF<FZ<II»NE»0> GO TO 612 

3 97 * 


K=K* 1 

396* 


IS SKI 3 

3 99* 


DO 611 jslvNH 

400* 


If IP1 tJ *E6 -01 GO 1C 6 11 

401* 


IF ( I • GT ■ J J A 5 U . JJ = AS ( J. I) 

402* 


ECfM31 = EC<Ki3!-AS|I»JI*GM0 0lj! 

403* 

511 

CONTINUE 

404* 

612 

CCMINUE 

4 05 * 

C 


406* 

c 

LCAD SYSTEM MATRIX PAP WITH AOO»ADK*AK* ELEMENTS 

407* 

c 


40 0* 


DC 23 Is 1 « 3 

409* 


DO 23 JSl .3 

410* 

23 

A (I * J1=A0Q i It JJ 

411* 


DO 24 1 = 1 1 3 

412* 


K =0 

413* 


DO 2 4 J = 1 tNH 

41 4 * 


IHP1 IJI.NE.or GO TO 24 

415* 


K=K* 1 

416* 


A lK*3t IlsAWiJt 11 

417* 


A< I tK*3»=AV 1 JtU 

41 8 * 

24 

C (KTINUE 

419* 


K= □ 

4 20 * 


DC 250 1=1 »NH 

4 21* 


IF<PIIZ!*NE*fl! GO TO Z5Q 
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422* 


xrxn 

4 2 3 * 


L- Q 

424* 


D ( 25 J=1»NH 

425* 


IFIPIfd I.NE.Ot GO 10 25 

426* 


L :L*1 

*27* 


IF IK .GT .L > GO 10 2 6 

*28* 


A 1 K *3 • L ♦ 3 ) ~ASI ItJI 

*29* 


GO TO 25 

* 30 * 

26 

A < K * 3 i L + 3 1 =AlL*3iK«3) 

431* 

25 

CONTINUE 

432* 

250 

CCMINUE 

433* 

C 


43 4* 

C 

S C L V E STSltF F! ii T R IX FOP REFERENCE EDO* A N G • ACCELERATION AND HINGE 

4 35 * 

C 

{RELATIVE) ROTATIONAL A CCELER AT ICNS 

436* 

C 


4 3 7* 


CALL S0DUfV«lV»EC*V<l«$92iURK1 

438* 


DC 910 Jr\U4 

4 3 9* 


K- J- 3 

4 4 0 * 


IFIPI IK) *N L .0) GO 1C 911 

441 * 


EC < J) = EC1 IV) 

4 4 2 * 


IV = IVM 

44 3 * 


GC TC 910 

4 4 4 * 

911 

EC* J ) = GMOO<K ) 

445* 

31 U 

CCMINUE 

446* 

92 

CONT IN LIE 

44 7* 


RETURN 

4 4 8* 


END 
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Appendix B 

Summary of MBDYTL, MBDYL User Requirements 

Subroutine Entry Statements 

CALL MBDYTL(NH,H,MB,MA,PB,PA,G,PI) 
or CALL MBDYL(NH,H,MB ? MA J FB > PA,G,PI) 

CALL MRATE(NH,TH,TB/TA ? FB,FA ? GM,GMD,GMDD,WO,WDOT) 

Input/Output Variable Type and Storage Specifications— Fortran V 

INTEGER NH, H(«), PI(») 

REAL MB(7), MA(n,7), PB(n,3), PA(n,n,3), G(n s 3), TH (n), 

TB(3), TA(n > 3), FB(3), FA(n,3), GM (n), GMD(n), 
GMDD(n), WO (3) 

DOUBLE PRECISION WDOT(n + 3)(where n - number of sys 

tem hinges) 

External Subroutines Called 

AINVD — double precision matrix inversion subroutine; user-supplied (see line 
224 in MBDYTL listing, line 223 in MBDYL listing) 

This subroutine inverts the matrix A, where A is of order IV X IV, 
IV ^3. The result is returned in A, 

Subroutine Setup 

Insert the Fortran V statement (see listings) 

PARAMETER Q = n 
where n = number of system binges 
Data Restrictions 
n — 1 

Core Storage Required 
Code: 1500 words 

Data: 500 words (minimum; increases with n) 

Listings 
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S J a 5 ? OUT IN E M 9 DY T L I N tf » Hi M9*MA*P3iPAiGtPl \ 


1 * 

2* C 

3 * C ADJUST A9L E 0 IMEN SIGNS 


4 ♦ 

5* 

6 * 

7 * 
8 * 
9* 
20 * 
12 * 
12 * 
13* 
14* 
15* 
ie* 

1 7* 

1 8 * 

1 9* 
20 * 
21 * 
22 * 
23* 
24* 
25* 
26* 

2 7* 
28* 

2 3* 
30 * 

n* 

32* 

31* 

34 * 

35* 

36* 

37* 

39* 

3 3* 
40 * 

4 1 * 
42* 
43* 
44* 
45* 
4 E * 
4 7 * 
4 8 * 
49* 
50* 
51 * 
52* 
53* 
54 * 
55* 
56* 
57* 
58* 
59* 
60* 
si* 
62 * 
63* 
64* 
65* 


C 

INTEGER Hll ) .PH n 

REAL MBI 1 » tHA ! NHilT I iF B tNH • 3 1 aPMNMt KH »3 1 1 S 1 NH» 3 > 

PARAMETER 0 = 4 

PARAMETER S = QO*V"Q*3*V«*::4*VfS3 = 3*S 
C 

C ADDITIONAL DIMENSIONED VARIABLES 

C 

DOUBLE PRECISION AfViVIftlRH I V 4 1 i B M A S 5 fSl 

DIMENSION ?HIS*3t3i*CXIS»S» • C ¥ 4 S • S » * 02 <S *S I t DX 0 < S « 5 I t OY 0 I Si 51* 
S 0 20C5'S]»G<HQi3t»nif5»3«3!!LMS»St'LVIS«S>iUflS’SliFEX0iSI’ 
tF5yOISJfF£2£HS)f!XOISI«noiS7frZOrS)*X9l3«3l* 

fCEf3lvAXSI3>« PSfS»Si3*3lftASIB»QliAV1Qi3>t AOC t 3 i 3 ) 

INTEGER KK19 ) »EP S < Q * S 1 

REAL LX*lY*L2riyX(SJrIYYtS>ilZZIS» f IXYTSUlX2«SUnZTSI 
EQUIVALENCE IPSiA>fllX.QXOJfKYiCY01f<L2*02GUlPHiTI 
N9 - NM ♦ 1 
C 

C INITIALIZE EPSlKtUT WITH ZEROS AND DEFINE KMK) 

C 

00 85 H=JtNH 
KK< < >=< *3 
00 85 J=1 » NB 
as EPS(KiJJ=0 

c 

C DEFINE EPSU.J) USING I J ) 

C 

DO 86 K = 1 »NH 
DO 96 J - 2 ♦ N B 

IF<<*EQ«IJ-1>1 EPS1K. Jl = 1 
IF i K.L 7 . i J-l M 6Q 70 8 7 
go io as 
07 CONTINUE 

JC=<* 1 
Jl= J-I 

00 3 3 L = JQi J I 
IFfK'GT.tL-)>> GO TO 89 

X = n CPS IK «. I . £Q i 1) .A ND • « HU- n • EQ« I L - 1 J I I EPS ( K • J l - 1 
89 CONTINUE 

86 CONTINUE 

C 

C INITIAL CALCULATION Of 3 A 3 YCENJER VECTORS W.R.T, BODY C,G.S 

C AND HINGE F 0 I NT S 

C 

1 XX U ) -MB I 1 1 
IY Y< 1 )iM3T2 I 
XZZUI=HBI3l 
IX Y ( 1 ) - M a 14 ) 

IXZ H 1=MB«5I 
I Y Zf 1 i:N8lfil 
BMA5SU l-MBI? » 

IHSBMAS S 1 1 1 

DO 35 J=2.NB 
IX XT J T A I J - 1 ill 
m i J) =MAI J-X *2 > 

12 21 J ) r M A t J - J i 3 J 
I XY I Jl =MA l J-l *4 1 
IXZU J=KAC J-l#5 > 

IYZI JlrNAl 

BM ASS f J 1-MA ( J- 1*7) 

35 TM-TM+0NASSCJ1 
DO 149 Irl.NB 
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66 * 


n=i-i 

67* 


CO 1 *• 3 J=1*NB 

6 9* 


J1=J-1 

65* 


IFtl.EG.J) GO TO 1E3 

70* 


IFtl.CT.J) GO TO 70 

71* 


IF tl.EO.l) GO TO 80 

72* 


TFt EPStll. J I.EG.l ) GO TO <*00 

73* 

70 

LXII.JIrFAflltllfl) 

74* 


LYIT. J)=PA(I1.I1»2) 

75* 


LZ (I > J 1 rPA ( 1 1 *1 1*31 

76* 


GO TO 14 9 

77* 

400 

CONTINUE 

78* 


00 600 K=I»J1 

7 S* 


IFtEPStKnJI.EQ.l) GO TC 5CC 

sq* 

600 

CONTINUE 

81* 


GO TO 149 

82* 

son 

LXt I* J)=PAt Il.K.l > 

63* 


mi.JliPMIlrK.H 

84* 


LZ< I * J ) =PA [ I1.K.3) 

85* 


GO TO 149 

86 * 

80 

00 90 L=1 • J1 

87* 


IF tEPStL*Jl.EG.l> GO Tc 101 

83* 

90 

CONTINUE 

85* 


GO TO 149 

90* 

101 

LXt I. J»=PB(L.l » 

91* 


L Y tl »«J ) ;FB 1 L * 2 > 

92* 


LZ(T» J)=PB(L« 3 > 

S3* 


GO TO 149 

94 * 

163 

LXt I» J 1=0. 

S5* 


LY (I « J ) = U. 

96* 


LZt It J)-0. 

S7* 

149 

CONTINUE 

93* 


DO 13 N=ltN3 

99* 


DO 13 Jri.NB 

100* 


DXt N* J 1 -l_ X T N? J ) 

1C1* 


DY tH. J)rLY(N» Jl 

10 2 * 


DZ ( N* J ) =LZ t N» J 1 

113* 


DO 13 K-1*N8 

104 ♦ 


DXt K*J)=DX!N.J)-lSMASStK)/T'f)*LX(N.K> 

its* 


DYCN.v’l=OY tN. j>- tBwASS CK>/TKl*LY (N»K 1 

106* 

13 

OZt N* JI=OZ<N» J 1-tGHASSt K ) /TMJ *LZtN*Ki 

107* 

C 


103* 

C 

CALCULATION CF AUGMENTED INERTIA DYADICS FOR EACH BODY 

165* 

C 


110* 


DO 31 N=1,N8 

111* 


PHtN.l.llriXX |IO 

112* 


PHt Ni 1*2 l=-IXY INI 

113* 


PH(Ntl,3J=-IXZCNI 

114* 


PHtN*2»2)=IYYf N) 

115* 


PHtN»2»3 J = -IYZtM 

116* 


PHCN*3.3 )=IZZtNI 

117* 


DC 3D J=1.NB 

11 9* 


PHIN*1*1)=PHIN*1*1> *3 MAS St JJ * 10Y t N* J)**2+DZ( N* J) **2 ) 

119* 


PHtN»I»2)=PHtN*l>2)- 6 MASS ( J > *0X ( N» J) *0 Y t N * J > 

120* 


PHtN *l»3)=PHtN»l*3)-BMASS(J)*DXtN*Jl*DZtN»J> 

121* 


PHtNf 2.2 >=PHCN« 2.2 ) *B MASS ( Jl» tOX ( N* J 1* *2*0Z tNt J ) **Z ) 

122* 


PHtN*2*3l=PH<N*2.3)-BMASSt J)*DYt N* J)*CZ(N. J1 

123* 

30 

PHlN,3*3>=PHtN*3»3J+BMASStJ)» f DX ( N * J I* *2 + C Y < Nr J > ** 2 I 

124* 


PH<N*2»l)=PHINtl*2) 

125* 


P H 1 N • 3 • 11=PH(N *1*3 J 

126* 

31 

PHt N * 3 * 2 ) =PH l N * 2 * 3 ) 

127* 

C 


128* 

c 

ADDITIONAL AUGMENTED INERTIA DYADICS IIN BODY I FRAME) 

129* 

c 


130* 


DO 751 J=1*NB 

131* 


DO 751 M = 1 * 3 

132* 


00 751 N=1.3 

133* 

751 

PSt J»J*M»N)=PH< J.H»N » 

134* 


DO 37 I = 1 * N 3 

135* 


DO 37 J=1*N6 

136* 


IFII.EQ.J) GO TO 37 
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137* 


PS < I* Ji 1 * 1 >r-TK* <DY C 1» J>*DY <Jt 1 1 -tDZ fl» J)*CZ f 0*1 )) 

133* 


PSITtJ»l»2)"TK*0XtJ*I>*0Y<XfJ) 

1 3°* 


PSlI.Jt lt3)'TP*0X f J»T> *CZ Cl* J! 

140* 


PSlIt Jt2# 11=TM*0Y< Jtl J*DX1I tJl 

mi* 


PS<I*J*2f2>=-TK* IDXIIp JI*DX CJ*IHDZII* J)*OZ CjrT 1 1 

142* 


PS( I* Jt2t 3)rTM *DY ( J ♦ 11 * DZ < I • J 7 

143* 


P SfI*Jt3*l)rTH*DZCJ*I>*CX(I«J> 

144* 


PStItJt3*2)rTM*CZ<JtI)*0YlI*J> 

146* 


PS CltJt 3 t3 lr-TK* COX ( I. Jl *DX C J? I ) *CY < I. J)*DY t J*I M 

146* 

v 37 

CONTINUE 

147* 

C 


14 8 * 

c 

ADO MATRIX ELEMENT COMPUTATION (3X3) 

143* 

c 


150 • 


00 3 I - 1 1 N 8 

151* 


DC 3 J- 1 » NB 

15 2* 


AGO tit 1 1 - AOO 4 1 1 1 J«l*11 

153* 


AGO <1*21 "ALUt It 2 1*PS (I* Jtl#2 > 

15 4* 


AOOt lt3) = A00tl t3)*PSt It J tit 31 

155* 


AOG«2*2)=AOCi(2»2l*PS (1« J*2»2V 

15 6* 


ADD ( 2*3 )“AGO 12 .3 1 *?S<It Jt2#31 

157* 


AOCC 3t3) = ACCK3t 3 J*PS (It Jt 3t 31 

15 5* 

3 

CONTINUE 

159* 


AOL'CZtllzAGCC It 2) 

ISO* 


AGO t 3 » 1 J - AGO 1 1 »3 > 

1 G 1* 


AOG < 3 ? 2 ) - AGO C 2 t 3 1 

16 2* 

c 


163* 

c 

AOK VECTOR ELEMENT CONFUTATION <3X11 

164* 

c 


165* 


DO 7 K - 1 1 NH 

16 6 * 


AVI K * 1 1 -0 ♦ 

167* 


AV lKt2) -0* 

16 3* 


AV t Kt 3 1 -U > 

163* 


DO 7 1=1 *N8 

170* 


00 7 J-l t N3 

171* 


IF (EPS CKt J1 „EG.t 1 GO TC 7 

17 2* 


00 10 Mri.3 

173* 


DO 1G L- 1 * 3 

174* 

10 

AVt K*M)-AVlKtM) + PSt ItJtM tL)*GtK»L J 

175* 

7 

CONTINUE 

176* 

C 


177* 

c 

AK M SCALAR ELEMENT COMPUTATION 

179* 

c 


173* 


DO 14 K - 1 * N H 

130* 


DO 14 M*1.NH 

181* 


IF (K *GT GO TO 14 

132* 


AIStll-0. 

183* 


AISt21-C. 

1 84 * 


AIS < 3 ) =0 « 

185* 


DO 15 JrltNB 

186* 


DO 15 I=1#N3 

187* 


IF n EPS (K *EQ »G1 *0R • IE PS IMtll «EQ »6) 1) GO TO 15 

183* 


DO 13 N=l»3 

189* 


DO 19 L' 1 * 3 

190* 

19 

AIS(Nl=AIStN )*PSt Jf I*N*LI*G< MtLl 

191* 

15 

CONTINUE 

192* 


AS< KtMl = GtKf D*AlSU)*GtK»2)*AISt2)*GtKt31 *AISt3 ) 

193* 

14 

CONTINUE 

134 * 

C 


195* 

C 

LC AG SYSTEM MATRIX < A 1 WITH AOOtAGK*AKM ELEMENTS 

196* 

c 


197* 


DO 23 Irlt3 

199* 


DO 23 J - l *3 

199* 

23 

AlI*J)rAOG<It 

200* 


DO 24 1-1*3 

2 Cl* 


K - C 

202* 


IV-3 

263* 


DO 24 Jr 1 1 NH 

20 4* 


IF( Pit J1 .N£,0* CO TO 24 

2 65 * 


KrK+1 

206* 


AtK*3*I)=AV< Jtl > 

207* 


AUtK + 3) = AVf Jtl 1 
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20 2* 

24 

CONTINUE 

219* 


Krf! 

210* 


DO 250 1= 1 ♦ NH 

211* 


ir<Pl CIl.NF.tj} GC TC 2 tr 

212* 


K=K* 1 

213* 


IV -TV 4 1 

214 * 


L=0 

2 15* 


00 25 JrjpNH 

216* 


IF( PIl JI.N5.U) GO TO 25 

217* 


LrL*l 

219* 


IF < K • GT*L 1 GO TO 26 

219* 


A t K 4 3 ■ L + 3 )-AS fl *J> 

22 0* 


GO TC 25 

221* 

26 

AfK*3*L43)rAtL*3*K*3) 

22 2* 

25 

CONTINUE 

223* 

250 

CONTINUE 

22 4* 


CALL AINVOf A* V*IV*T 1099* WRKl 

225* 

1099 

CONTINUE 

22 6* 


RETURN 

2 27* 


ENTRY NRATciNHiTHfTe .TA»FB i F A * 6 M*GMD * GMOO *WC »EQ 1 

22 9* 


DIMENSION TO <3 ) *TA«nH. 3> *FB(3> *FACNHt3 >* GM<1 ) « GMD < 1 )»GM0DI1) . 

229* 


*THCNHlfWt(3).E(S3pl> 

230* 


DOUBLE PRECISION £C(Vlt£Qm 

231* 

C 


232* 

c 

8ODY-TO-SO0Y COORDINATE TRANSFORMATIONS 

2 3 3* 

c 


23 4 * 


DO 335 J=1*NH 

2 3 f * 


K -H ( J ) 

236 * 


A 8 < It 1 )=1* 

237* 


ABU*2) = GM (JMG fJtZI 

238* 


A 8 ( 1*3)=-GHI J1*GJJ*21 

239* 


AB f2*l>r-A81 1*2) 

240* 


A 8 ( 2*2 )ri. 

241* 


ABC?*3) -GM C J)*G f J* 1 t 

24 2* 


A3 ( 3*1 ):-A3(l»5) 

243* 


ABC3v21=-Aei2«II 

24 4 * 


A B ( 3 * 3 > - 1 * 

2 4 5* 


IFCK.EO.O* GC TO 3356 

246* 


00 334 L -1 * 3 

2 47* 


DO 134 K = l*3 

24 3* 


TU.J+M»iM:a. 

249* 


DO 334 1=1*3 

250* 

334 

T< 1 . J+l.Li M )-TU * J*ltL*M >+A9 tL *1 )*T< ltK + l.I»Ml 

2 51* 


GO TO 335 

252* 

3350 

CONTINUE 

2 53* 


DO 3351 L= 1 * 3 

254 * 


DO 3351 M=i*3 

255* 

3351 

T ( 1* J*1 *L*M)r A6 CL* *) 

258* 

335 

CONTINUE 

257* 

C 


25 8 * 

C 

COC RD * TRANSFORMATION OF G VECTORS (TO REF • BODY FRAME ) 

259* 

c 


26 0 * 


00 362 1-1 * NH 

261* 


DO 362 J = 1 ► 3 

26 2* 


Goa#j)=u. 

2 €3* 


00 3S2 Kr l * 3 

26 4* 


GOC I*J)“GO( I* J ) 4 T1 1 rX + l * K, J> *GtIt K) 

265* 

362 

CONTINUE 

266 * 

C 


267* 

c 

EXTERNAL FCRCESiEXTEFNAl torques 

26 3* 

c 


2 69* 


FEXO CU = F9( 1) 

270* 


FEYQ<1)-F8(2) 

271* 


FF2C C11-F2C3) 

272* 


TXO(l)=Ta(l 1 

2 73* 


TYC (1 J-TB ( 21 

274* 


TZC(l)=Te(3} 

275* 


DO 17 I = 2 * N B 

27 6* 


Kcl-l 

277* 


FEXO ril-T fit I# I*lMFAfK*l> 4 T Cl* 1*2*1 )*FAIK*2 )*TUtI*3tl)*FA(K »3) 

279* 


FEYOm=Tllt 1*1 ♦2)*FAtK*ll*Ttl* 1*2*2) *FA(K*2) 4 TUtI* 3*2) *FAtK#3> 
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2 7 9 * 
280 * 
281* 
232* 
283* 
294 * 
265* 
236* 
287* 
2S9 * 
289* 
2511 * 
251 * 
252 * 
293* 
234* 

2 96* 
296* 
297* 
293* 
295* 
300 * 

3 01* 
302* 
303* 
30 4* 
306* 
306 * 
307* 

30 3* 
309* 
310* 
311* 
312* 
313* 
314* 
315* 
316* 
317* 

31 S* 
3 19* 

32 0 * 
3 21* 
322 * 
3 23* 
324* 
3 25* 
328* 
327* 
32 8* 
3 29* 
330* 
331* 
332* 
333* 
334* 
335* 
336 * 
337* 

33 S* 
3 32* 
340* 
341* 

34 2* 
343* 
34 4* 
3 45* 
346* 
347* 
34 9 * 
342* 


FE20 fI)=TU*I*lt3UFA«K*lUTUiI.2#3l*FAtK*2l*TCl*It3*31*FAtK»3l 
TX01 I) =T 11* If I » 1 ) #TA CX.11 MU tlf 2t 1 1 *TAtK»2 UT 11 tl t 3t 1 ) *TA IK* 3 > 
TYC«I»: Ttl'ltl»2)*TAfK*lt-»TCl'T«2*2ltTAtK’?)+T<l»I»3*2Y*TAn<*2l 
T7Q l 11“ Ttlt:*lf3)*TAlK.l)*TU»If2.3l*TA(K*2l*Tll*I.3.31*TA(Kt3> 
DO 17 J=1*N8 

DXO { I# J)=Tt It It 1 # X) *0X1 l*J!*T(l*I«2»n *0Y( I* Jl + T t lfI»3«11*DZtI.J1 
D Y0<1«J1::T U'X*1’2>*DX |I»jUT |1 *1*2.2 )«DY tI»J)+T 11.1.3.2 1*02 t It JJ 
DZO { IpJ)-T< L.I. l«31*DXt XtJ)*T(ltX.2*3 1 *DY t 1*3* 3 )*DZ<I*J) 
17 CONTINUE 

DO 367 I-lrNB 
DX C ? I.IlrDXll.I) 

oyo< uiudyu* n 

367 OZfltl*iI=o?Cl*I> 

C 

C VECTOR CROSS PRODUCTS DESCRIBING SYSTEM ROTATIONAL COUPLING. 

C 1 QUADRATIC TERMS INVOLVING THE CONNECTING BODY ANGULAR 

C VELOCITIES AND THE MUTUAL 6 A R Y CENTE R- HI NGE VECTORS) 

c 

DO Z3U N - 1 » N B 
CPX=0. 

C P Y-0 ♦ 

CPZ=0. 

DO 2301 Lil.NB 

CPX=CPX*DY0(N*L)*FEZ0<L1 -DZO <N*L)*FEYO<Ll 
CPYrCPY+D20<N»L)*FEXCtL>“OXO<N*LUF€Z0CL) 

CPZ - CP Z + C X 0 < N * L > *FEYO<L ) -0Y 0 { N * L ) * f E X 0 ( L ) 

2301 CONTINUE 

K = 3 * t N-l ) 

E(K + lfl1=TXCm+CPX 
E l K + 2 » nrTYOlN)*CPY 
E IX*3»1I=T20(M*CPZ 
230 CONTINUE 
C 

C COMPUTE CORRECTION ELEMENTS FOR C E) VECTOR 

C 

DO 55 MI-1 r 3 
55 EC INl)~E<Ml#lJ 

00 52 J=2*N3 
DO 52 M=l*3 
Kl-3*l J-1UM 

52 ECCHJ-CCCMUE ( K1»U 

1=0 

DO 60 KzltNH 

IF< PKK1.NE.01 GO TO 60 

1 = 1*1 

ECI 3 ) =U * 

DO 601 M = l*3 
601 CH M 1 -0 . 

DO 61 J=1*NB 

IF(EPStKtJ).EQ.Q) GO TO 61 
DO 65 Mzlt3 
Jl= 3 * 1 J- 1 ) 

65 CEtH>=CElMI+E<Jlf 1) 

61 CONTINUE 

DO 66 Lrl.3 

66 ECt 1*3 )r£C(X*3 1 + G01 K# L) * CEt L 1 
rCtl + 31 = ECfl43UTHCKl 

80 CONTINUE 

DO 610 1=1*3 
DO 610 J=1*NH 
IFlPieJUEG.O* GO TO 610 
£C!T>=EC!I>-AV: Jf IUGMDDl J) 

610 CONTINUE 

KrO 
IV =3 

DO 612 1 = 1 * NH 
IF(PIIIUNE.U) GC TO 612 
K=K* 1 
IV=IV*1 
DC 611 J=L*NH 
IFIPI«J).EQ.O> GC TO 611 
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350* 

351* 

35 2* 
352* 
354* 
3 55* 
356* 
357* 
353* 
3 55* 
360* 
361* 
362* 
3 E 3* 
364* 
3 65* 
366* 
367* 

36 3* 
369* 
370* 
371* 
372* 
3 73* 


IFt l.G7,J) ASIIfJ)=ASIJ*II 
F.CtK + 3> = E:C<K + 3F-AS fit jr*GMCC f J1 

611 CONTINUE 

612 CONTINUE 

C SOLVE SYSTEM MATRIX FOR REFERENCE BODY A NG * ACCELERATION AND FINGE 

C l RELA T TVE 1 ROTATIONAL ACCELERATIONS 

C 

00 671 I=1*IV 
rQ<if-0* 

00 671 J=1*IV 
EQ<lt=EGiXI + A fit J1*ECC Jl 
671 CONTINUE 

DO 910 J=V»4 
K=J-3 

IF *PI IK1.NE.D) 60 TO 911 
EOi JirCOdV > 

IV=IV~1 
GO TO 910 

Sll EG (Jl=GKDDfK > 

910 CONTINUE 
92 CONTINUE 
RETURN 
END 
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1 * 
2 4 

3* 

4 ♦ 

5 * 
S* 
?♦ 


c 

c 

c 


SUB POUT INE M30 VU K'H i H t MB » MA t PB tP A i Gi P I > 
ADJUSTABLE DIMENS IONS 
INTEGER Mm t PI lli 

REAL M B 1 1 ) » M A I N H 9 ? ) 1 p B I NHi 3) »PAlNHiNHfil « G t NH • 3 I 
PARAMETER Qztf 


8 * 

9* 

TO* 

n * 
12 * 

1 3* 

n* 

15* 

16* 

1 7* 

1 a* 

19* 

2 o * 

21* 

22* 

23* 

24* 

25* 

26* 

27* 

2 a* 

29* 

JO* 

31* 

32* 

33* 

34* 

35* 

36* 

37* 

3 9 * 

3 9* 
4C* 
<11 * 

4 2* 
4 3* 
4 4* 

45 * 

46 * 
4 7* 

4 8* 
49* 
50* 
51 * 
52* 
53* 
54* 
55* 
56* 
57* 
58* 

5 9* 
60* 
61* 
62* 
63* 
64* 


PARAMETER S = fl * 2 . V = Q * 3 • V 4 = 4 * V f S 3 = 3* S 
C 

C ADDITIONAL DIMENSIONED VARIABLES 

C 

0QU3lE PRECISION a (V f VI f WR K 1 V4J • 8MASS IS ) 

DIMENS ION PHlSi3*3liDXIS»S)iDYlS«SlfD2tSiS)i 
tLXt S tSJ *L Y IS tSh LZ <Sf SI * F E X IS I » F St IS I* F SZIS \ .TX I SI • IV I Sit T2 I SI, 
SCE13>'AIS(3>i PSIStSt3i31.ASfa.0l»AVIO#3>* AOG I 3> 3 J 

INTEGER KK 14 J vEP SOi 5 I 

REAL LXpLYrLZtIVX |S>ilVVIS>»lZ?IS>»IXVIS|»2XZrsi’IV2ISI 
EQUIVALENCE 1PS»LX»AJ , 

N6=NH*1 
C 

C INITIALIZE EPS ( K » J 1 WITH ZEROS AND DEFINE KKIKI 

C 

DO 65 K= 1 » NR 
KKt < l=K *1 
DO BE Jrj « MB 
85 EP SI K • J » r C 

C 

C DEFINE EPSK • J) US IN G HI JJ 

C 

02 86 < r i , nt ^ 

DO SE J-2fNB 

I C M.E3.U“JII EPS IK* Jm 
XF(KiLT.IJ-JI) GO TO 8? 

GO TO 36 
B 7 CONTINUE 

JO-K* 1 

00 09 L=JD»J1 
IHKrGlilLO)) GO TO 69 

I r II EPS I K ». I «EG. 1! iAN0« IHI J-ll ^Q.IL- 11 I I CPS IK 9 JI =1 
89 CONTINUE 

es CONTINUE 

C 

C INITIAL CALCULATION Cf 3A3YCENTER VECTORS U« ff « I • 800Y C.G.S 

C AND HINGE POINTS 

C 

ixx m -mb 1 1 > 

IV U 1 l=MBI2 1 

1 2 z n ) = m b m 

IX Y( 1) -MB 14 1 

1 X2 1 11 =MB l 5 1 
IY 211 ) = M8 (6 I 
BMASS m = MB I 7 ) 

TMI9MASSI II 

00 35 J - 2 t N B 
IXXI J ) -M A IJ -Itl I 
It Y (JJ =MA I J-l .2 k 
IZ Z( J I = M A < J - 1 ♦ 3 ) 

1 X Y IJIrMAI J-li4i 
IX 21 J I =M A I J - 1 <5 1 
I Y 2 (JliKAIJ-l >6) 

BMASS UUMA I J-l 1 7 I 

35 TM: TK* BMASSI JI 
00 1 49 I- 1 1 N 8 
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TO MOO 


65* 


11=1-1 

S 5 * 


09 3 M9 J = 1 .N3 

6 ?* 


J] = J-l 

6 8* 


I c t I •EQ.J 1 GO TO IS 3 

6 9* 


IH I.GT. Jl GO TO 7 C 

7 0 * 


I C 1 I .EOtl 1 GO TO 3 G 

71 * 


IF (EPS < 11 * J ) • E Q . I 1 GO To MOD 

72* 

70 

LXII*JJ=PAIJ3»Ilil) 

73* 


in i » Ji-Pft t n » 1 1 « 2 1 

7M* 


IZ( I i,JI =P4 1 II t 11 t 3 J 

75* 


GO TO 1 M 9 

7 5 * 

MOO 

COM INUE 

77* 


DO FC'C KsitJl 

78* 


l-i EPS1K.J J « EO « 2 1 GO TO 5Q0 

73* 

6C (i 

CONTINUE 

30* 


GO TO 3 M3 

61 * 

500 

LXI I* J) zPh UhKil) 

32* 


L V II . J UP A 1 13 * K t 2\ 

51* 


LZII*J1=PAII3 » K 1 3 t 

3M ♦ 


GO TO 1 M9 

9 5* 

ar 

0 0 90 U = 3*J1 

as* 


l-i EPS iUJI.fl.ll 69 TO 1C 1 

6 7* 

3P 

CONTINUE 

38* 


GO TO 1M9 

69* 

101 

LXI li J)=PB r L * 1 1 

9G* 


LY(I»J)=P3IL#2) 

91 * 


L Z 1 I 1 J) =PS 1 L » 3 J 

92* 


GO TO 1 M9 

33* 

163 

LXI N JCO. 

9M* 


mitjuc. 

95* 


L Z ( T t J ) =0 • 

96 • 

1 M 9 

CONT INUE 

97* 


DO 13 N=1 t NB 

99* 


DO 13 J = 1 * N 3 

99* 


DXINi JULX IN* J) 

1 QG* 


0 Y 1 N * J ) = L Y 1 N i J 1 

101 • 


02lk»JI=LZIN«JI 

1 C2 * 


DO 13 K = 1 iMB 

10 3* 


DXI Ni J1 = DX IN.JJ-loMASSlKl/TMl*LXtNfK) 

10 M* 


OY 1 N #*11 = DY IN • JJ - I3M4SSI K1 / IMJ * L Y 1 N • K 1 

105* 

13 

DZlN»J>=D2lN*J)-iBMfiSSlKWTMl*LZINtK) 

1 GS* 

C 


10 7* 

C 

CALCULATION OF AUGMENTED iNEpTIA OYAOICS FOR EACH BODY 

1 08* 

c 


1 0 9 * 


00 31 N = 1 * N B 

110* 


p-u n« i » n - ix x m 

111* 


PFMNil »2I = "IXY INI 

112* 


P H 1 N i 1 i 3 1 -« I X 2 J N » 

113* 


PHIN.2 i2UZWfN> 

in* 


PHI N i 2 » 3 1 = - I Y Z 1 N J 

115* 


PMCN*3*JI=IZZIN) 

US* 


00 JO J = 1 *N 3 

3 1 7* 


PBlfctl •lt = PH(N 1 1 » 11 * 6 M A S S 1 J)* I 0 Y 1 N * J 7 * *2 ♦ OZ 1 Ni J \ * *2 I 

118* 


PHI N * 1 i21=PH IN, 1 i2 1-3 MASS < Jl *0X1 Nt JhOTl Nt Jl 

119* 


PHI Nil t3lrPHlNtlt3)-BMASS 1 J 1 * Q X 1 N » J t « 0 Z 1 N » J I 

1 20* 


P-HN*2*2l=PHIN»2i2)+9MASil Jl* K 0 X INiJI**2*Q?<N#JI**2l 

121* 


PH(Ni?»3l-PHINr2i 37-BHASS T J 1 *0 Y 1 N * J 1 * D Z 1 N * J 1 

1 22* 

3 0 

P HI Nt 3* 1)=P HlNt 3 *3) *3 MAS 5 1 Jl* IOX 1 N. J I* *2*0 VI Ni Jl **2I 

123* 


PHI N.2 tlUPHIN t It?) 

1 2M* 


PHI Ni3ill=PHIN. 1*31 

125* 

31 

PHI Nt3 *27=PHlN*2t3) 

1 26* 

C 


127* 

C 

ADDITIONAL AUGMENTED INERTIA 0YADIC5 IIN BODY I FRAME 1 

3 2 9 * 

c 


3 2 9* 


DO 753 J = J *NB 

1 30* 


00 751 M=l*3 

131* 


00 751 N=l*3 

1 3 2* 

751 

P5U • J#M iNI =PH< JrM iN 1 

3 3 3 * 


00 37 I = 1 1 NB 

1 3M* 


00 37 J=1.NB 

1 3 5 * 


IF I r.EO. Jl GO TO 37 
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2 36* 


PSI I » J i 1 1 1 |r-7M*!Dy i Ir-Jl *DY IJ* I> ♦DZ!I*J>*D2IJ*I 11 

1 37* 


P S l T < J i Ji21-1M*0XlJiII*L)Vt I • J ) 

138* 


P S < IfJil i3 |:TM*DX(JfII*OZ(I»J) 

1 3 9* 


PSI I»J*2*lJ-TM*QY(Jf I I ♦ D X 1 I * J ) 

HO* 


PSI I » J t 2 t 2 I : - T M * t D X tI.JI*DXlJ.II ♦OZlI*JI*OZIJtI|l 

1 41* 


PSI I»J»?i3J=TM*0YI J*I)*0Z1I*J} 

19?* 


PS t 1 1 Ji 3 * 1 ) = T M * DZ 1 J i I 1 * D X I I»J) 

1 43 * 


PSI ItJi3i2>:TM*OZIJi I 1 * 0 Y 1 I . J J 

1 44* 


PSI IiJf3*3l=-TK*lDX 1 I « J) * 0 X 1 J« I J ♦OVIIf JI*OH J»I M 

145* 

3 7 

CONT INU £ 

1 4 £ * 

C 


14 7* 

C 

A D G MATRIX ELEMENT COMMUTATION 13X31 

148* 

c 


14 9* 


00 3 I - 1 . N 3 

3 50 * 


DO 3 J"1 *NB 

153 * 


AGCI 1 » 1) = ACC <1 f 1 ) *PS Ui Ji 1 • 11 

152* 


AQ0(l«2t=AGCIlt?) *PS!ItJpli2l 

1 51* 


A2CI 1 < 3 ) - ACC t 1 ■ 3 1 *P S < Ii Ji 1 « 3J 

154* 


AGG i 2 * 2 1 = A GDI ? • 2) *PS 1 It Ji 2 i2 1 

1 55* 


A 0 C 1 2.3l“ACGI2iJ )+PSI IiJ«2«3l 

156* 


AOOI3f3UACLI3i3MP5IItJ*3i3l 

157* 

3 

CONT INUF 

156* 


ADO 12 » 1) =ADOfl • 21 

159* 


AGCt 3.11 = AGO t I i 3 J 

160* 


ACC f 3»?>=AGOt2« 3» 

1 £1 ♦ 

c 


162* 

c 

ACK UECTGR ELEMENT COMPUTATION 13X11 

163* 

c 


164* 


00 7 K = 1 i IV H 

1 £5* 


AVM i 1 1 =G. 

1 66 * 


A V ( K * 2 3=0. 

1 67* 


AVIKi3 1=G • 

3 6 8 * 


00 7 r:l.NB 

1 69* 


00 7 J: 1 i.Nd 

1 70* 


IF 1 EPS IK. J 1 ,EQ.O) GC TO 7 

3 71* 


DO 1G K-l .3 

172* 


DO ID L= 3 • 3 

3 7 3 * 

10 

AVI < .Ml =AV U .Ml *PSIIiJ'M«LI*GtK«U 

1 7 4* 

7 

CONTINUE 

2 7 5 * 

c 


1 76* 

c 

A KM 5CALAR ELEMENT COMPUTATION 

3 7 7 * 

c 


178* 


00 14 K: I i NH 

179* 


00 1 4 M = 1 iNH 

I6G* 


IFCK*GTtM) GO TO 14 

1 31 * 


flISM 1 - D» 

132* 


AIS 12 l:C. 

1 83* 


AISI 31-0* 

3 64 * 


DO 35 J=1.N9 

i as* 


00 35 1 = 1 * N 9 

186* 


IFC (EPS tK» JI .EG.Q I .OP. (EPS IH«I 1 .EQiO M GO TO 15 

1 87* 


00 13 N - 1 i 3 

163* 


DO 18 L - 1 p 3 

1 8 3* 

13 

AISCNI = AISINM?S1JiIiMfU*6CH.L) 

190 • 

15 

CONTINUE 

1 91* 


AS! K f M) = G M . 1 J * A 1SI1MGIK.2I* AIS 1 21 *G<X«3)»AISI3I 

2 32 * 

14 

CONTINUE 

1 93* 

C 


194* 

C 

LOAD SYSTEM MATRIX IU WITH AOG.AGKiAKM ELEMENTS 

1 95* 

C 


1 96 * 


DO 23 1=1-3 

1 97* 


00 23 J = 1 .3 

193* 

23 

A II rJ) = AGC 1 I. J) 

1 99* 


DO 24 1:1*3 

2D 0 * 


K =G 

2C1* 


IV = 3 

20 2 * 


DO 24 J: 3 » NH 

203 * 


IFCPlfJI.NE.OI GO f3 24 

20 4 * 


K =K ♦ I 

205 * 


A! K* 3 « I 1 = AV I Jt 1 1 

20 6 * 


A II«K*3I=AVIJ» 11 
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2 0 7* 

24 

CONI INUE 

20 8 * 


K=D 

209* 


00 250 I=J#NH 

210* 


IMFItll.NE.Gl GD TO 25D 

21 1 * 


K = K* 1 

21 2* 


X V r I V * ] 

213* 


L = C 

214* 


DO 2 5 J - 1 i N H 

215* 


IMPIU3.NE.C) GO TO 25 

2 1 € * 


L - L ♦ 1 

2 1 7 * 


IMK . G 1 , L 1 GO TO 26 

21 8* 


A <K*3iL*3l =AS ( IiJ) 

213* 


GO TO 25 

22 0 * 

2 6 

MK«3'K*)=AIL43«K43I 

221 * 

25 

C 0 n r INUE 

222* 

25 0 

CON I INUE 

2 2 3 * 


CALL AINVOI«*V*IVi$lG93fU3TKI 

22k* 

10 39 

CONTINUE 

2 25 * 


RETURN 

226* 


ENTRY HRATEINMfTHiT6rlA»FBfFAi GM * G MD t GMOD » U G i E G 1 

2 2 7 * 


3 IMENS ION t B 131 i U IN Hi 31 i F9 t 31 > F A 'UHf 3 1 *:GN4 1 1 «GMQ11 li GMQOt 1 l» 

'22 8* 


$1H(fvHliiiOI5)iEtS3il ) 

223* 


0 0 U 9 L E PRECISION EClVIiEQIVI 

2 30 * 

C 


231* 

C 

EXTERNAL FDRCEStEXIERNU T OR Q U£ S * HI NOE TORQUES 

232* 

C 


2 3 3 * 


FEXf 1 J - f 3 II ) 

234* 


FEY ill-FBl 21 

2 35 * 


FEZ! 1 ) = F 9 1 3 1 

236* 


TXtl>”TBtl ) 

2 3 7 * 


T Y M 1 = T G \ 2 1 

23 8* 


TZtl 1 “ T B ( 3 ) 

2 39* 


DO 3 3 J = 2 i N B 

240* 


TX4 JI=TAI J-l *1 ) 

241 * 


TY<Jl-TAtJ-I *21 

242* 


TZIJ)=TA(J-1»3 I 

243* 


FEX(J):FAIJ-3fl> 

244* 


FEY ( J]zf At J-l .2) 

24 5 * 

33 

FEZ < J)=FA| J-l .3) 

245* 

C 


24 7* 

c 

VECTOR CROSS PRODUCTS DESCRIBING SYSTEM ROTATIONAL COUPLING. 

2 4 8* 

c 

{QUADRATIC TERMS INVOLVING THE CONNECTING a ODY ANGULAR 

24 9* 

c 

VELOCITIES AND THE MUTUAL BA RY C6 NTE R-HI NGE VECTORS) 

250* 

c 


251 ♦ 


00 23C N = 1 » NG 

2 52 * 


CP X: G . 

253* 


CPY=D. 

2 S 4 ♦ 


C P 2- C • 

25 5* 


DO 2301 L=ItNB 

256* 


CPX=CPX«OVIM*LI*fEZUI-DZ<NiLI*FETfUI 

25 7* 


CPY-CPY*D2 INiL )*F£X (L ) “DX t N « L 1 * F E Z IL 1 

258* 


CPZ=CPZ*DX<NiL>*FEY!L>-DYIN*U*F£XILI 

Z 5 B ♦ 

23 01 

CONTINUE 

260* 


K s 3 * t N- 1 I 

261 * 


E JK + 1 * 1 UIXfNI *CPX 

2 S 2 * 


E( K* 2,1 UTY IN ) *CPY 

26 3 * 


E 1 K ♦ 3 1 1 1 = TZ(NI * CP ? 

264* 

230 

CONTINUE 

265* 

C 


266* 

C 

COMPUTE CORRECTION ELEMENTS FOR IE) VECTOR 

26 7* 

C 


2 6 8* 


DO 55 MI- 1 i 3 

26 3* 

55 

ECmUMPi Itl ) 

2 7 0* 


DO 52 J = 2 i N B 

27 3 * 


DO 52 M=]»3 

2 7 2 * 


K1=3*IJ-JI*M 

2 7 3 * 

52 

ECm =ECIM) «E< KI' 1 J 

2 7 4 * 


I- G 

2 7 5* 


DO EG K=ltNH 

2 7 6* 


IF < PIIK )iN£.Q) GO TO 60 

2 7 7 * 


1=1*3 
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2 73- 
279’ 

1 ac« 
281 ■ 

2 92 - 
283- 
2 04- 
20 5 1 
2 35 1 
28? 
288- 
289 
2 30 
29] 

2 92 
293. 
294- 
295 
2 95- 
29? 

2 99 - 
293 

3 CO - 
30 1 
302- 
3C3 
3C4 
305 
305 
30 7 
3ca 

30 9 

310 

311 

312 

313 

314 

315 
315 

31 7 
31 0- 

31 9 
3 2Q 1 

321 

322 

32 3 
324- 
325 
325 
327 


EC* I ♦ 3 1 - C • 

00 5 01 M = 1 » 3 
501 CEItf)=Q. 

00 61 J=lf*B 

I? i EPS M • J ) • EG • C I 6J TO 61 
00 65 1 » 3 

Jl = 3* t J-l 1«* 

6 5 CE^):C£{MHCiJlili 

51 CONTINUE 

00 E6 1=1*3 

56 EC1I*3)=ECII*3WG<K«U«CEIL) 

ECCI + 3 I = E C I I ♦ 3 1 +T *H K l 
60 CONTINUE 

DO 610 1=1 .3 

DO 510 J= 1 *NH 

IfiPi Ul .EO.G) GO 10 610 

E C * n = £C< IT- AVUtll’GMQDI JI 

610 CONTINUE 
K = C 

1 V - 3 

00 512 1=1. NH 

IF I PI I II .WE .D P 60 TO € 1Z 

K -K+ 1 

I V = I V +3 

DO 511 J= 1 »N H 

IF<PI f Jk.EQ.QI GO TO Ell 

I'< I • G7 mJ l AST It JlrAS i Ji II 

ECIK*3l=ECIK<3)-A5IXtJ)*GMDDIJ) 

611 CONTINUE 

612 CONTINUE 
C 

C SOLVE SYSTEM MATRIX FOR REFERENCE & DO V ANG « ACCELERATION AND HINGE 

C IRELA1IVE1 ROTATIONAL ACCELERATIONS 

C 

aa s?i i = i. iv 
EaiU=o. 

DO 671 J=2 i IV 
EGt I iztQiil hM!,J»*eC! JI 
& 7 1 CONTINUE 

OC 910 J = V .4 
K = J- 3 

IF IPI no .NE .0 I GO TO 911 
E9 I J 1 = EO I IV) 

IVr IV-1 
GO TO 33 G 

311 EG* J>=DMODIK> 

910 CONTINUE 
92 CONTINUE 

RETURN 
END 
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Appendix C 

Summary of MBDYPL User Requirements 


Subroutine Entry Statements 

CALL MBDYPLtNH^MB^A^PAAPIMTI) 

CALL MRATE(NH ) TH,TB,TA ) FB,FA,GM,GMD,GMDD > WO ? WDOT) 

Input/Output Variable Type and Storage Specifications— Fortran V 

INTEGER NH, H (n), PI(n), LI(n), TI(n) 

REAL MB(7), MA(n,7), PB(n,3), PA(n,n,3), G(n,3), TH(n), 

TB(3), TA(n,3), FB(3), FA(n,3), GM(n), GMD(n), 
GMDD(n), WO(3) 

DOUBLE PRECISION WDOT(n + 3)(where n ~ number of sys- 
tem hinges) 


External Subroutines Called 


1. AINVD—double precision matrix inversion subroutine; user-supplied (see line 
272 in MBDYPL listing) ' 

This subroutine inverts the matrix AT, where AT is of order NMT X NMT, 
NMT^L The result is returned in AT. 

2, SOD~double precision matrix equation solving subroutine; user-supplied (see 
line 666 in MBDYPL listing) 

This subroutine solves the matrix equation Ax — EE, where A is of order 
NLXNL, NL^3. 

Subroutine Setup 

Insert the Fortran V statement (see listing) 

PARAMETER Q = n 
where n — number of system hinges. 

Data Restrictions 
n — 1 

Core Storage Required 
Code: 3000 words 

Data: 1000 words (minimum; increases with n) 

Listing 
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1 * 

2 * 

3 * 
4* 
5* 
6 * 
7* 

8 * 

9 * 
10 * 
11 * 
12 * 
13* 
14* 
15* 
16* 

3 7 * 

1 0* 

1 9* 
20 * 
21 • 

2 2 * 
23* 
24 * 
Z 5 + 
26* 
27* 
28* 
2 3 * 
30* 
31 * 
32* 
33* 
34* 
35* 
36* 
37* 
38* 
39* 
40 * 
41* 
42* 
43* 
44 * 
45* 
46* 
47* 
48* 

4 9* 
50* 
51* 
52* 

5 3* 
54* 
55* 
56* 
57* 
5 0 * 
59* 
60 * 
61* 
62 * 
S3* 
64* 


SUa^OUT INE tf BDf P MNHi H« M8t HA « P8f PA'G«P1«U «T Z1 
C 

C ADJUST A9LE 0 IWEN S10NS 

C 

INTEGER HUItPimdimiTlIl) 

REAL MBm>MAINHi77.PB(NHi31iPMNH*NHi3)tGINHt3| 

PARAMETER Qr4 

PARAMETER U = Gnl*V=Q*3tU4r4*V»S3-3*U 
C 

C ADDITIONAL DIMENSIONED VARIABLES 

C 

DOb E-L i PRECISION A I V , V > * WR* I V 4 1 i B M A S S l U 1 
D0U3LE PRECISION A 11Q «fl> « A E < V * V » « EC IV) » At! V • V 1 
DIMENSION PSIUrU»3i 31 . DXIU.UJ# QY t U 9 U ) t 02 t U *U I » 

$ L X I U • U 1 jLY{U*U)iL2IUfU) »PHTU#3*3I 

DIM ENS ION ABI3«3) #CEI31tCW <31 • A I S I 3) * E A 1 3 ) ♦ GO I Q * 3 I ■ AOO I 3 • 3 1 • A V i $ *3 
S 1 • AS lQ#a J f 7 I I #Uf 3# Jl • BA I3t 31 » AS 8 I Of 3 J I DX OlOfb 7» 07 01 U# Ul« oza tu* # 
$TX0tU>iTY0tU)tTZCIU)»FLX0tUl«FEYQtU7«FE20tUk 
DIMENSION G 1 S 10) fG2SIQ)«G35<aj»G12.IQJ»G13IQli&23<QI 
INTEGER KK IG) * EPS 19 *U J pR » S > 7 AH I Q> » 1 ANF « 0 » * NUNF t U I • 7 AB 
REA, LX*LViL2#IXXfUl i U V < Ul * 122 I Ul p I XY 10 I.IXZIUI* 1*2 tUI 
EQUIVALENCE UiPS$»ILX>OXOtASI»lLr«bVOIvlLZtOZOI 

N9=NH* 1 
INT-V 

00 SC6 K = 1«NH 
606 LISUH:LISUM*L 1 1 K 1 
C 

C INITIALIZE EPStKfJ) WITH ZEROS AND DEFINE KK IK I 

C 

DO B 5 K-lfNH 
K K I < > — K * 2 
DO 85 J- 1 • N 3 

85 EPSIK#J)-Q 
C 

C OIF INE E P S I K p J I US IN G H < J1 

C 

DO 96 Krl #N H 
00 86 JrZiNB 

X. e M«EQ«4j-l J J EPSIKiJJ-l 
IF I K • L T . I J-ll J GO TO 8 7 
GO TO 86 
87 CONTINUE 

JQ-K*1 
Jl= J-l 

00 0 9 L - J G i J 1 

IF IK.GT. tl-U I GO TO 89 

IF t < EPS IK #i. J «EG * 1) «A ND* IH< J-l ) < £Q» I L- 1 II) EPS IK t J1 =1 
09 CONTINUE 

86 CONTINUE 
C 

C LOCATE POTENTIAL TERMINAL APPENDAGE BODIES 

C 

NN - 0 
M=0 
LI -Q 

805 CONTINUE 

00 809 J- 1 iN H 
XFIHIJ>.EQ«MI GO TO 8D0 
GO TO 809 
608 LL-LL *1 

TAMCLLI-J 
80 9 CONTINUE 

DO 910 L 2 1 • N H 
K=T AM 7L I 
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65* 


IF ! T AM T L > .EG .0 ) GO 10 3 10 


se* 


NP I -fl 


6 7* 


NE P- C 


68* 


00 eC7 U=2.NB 


Q 9* 


1MEPSIK.J) .60,1 > NEP-NEPM 


70* 


IF 1 ILI 4 J-l > «EQ .1) .AND ■ UP5 («» J> .E9.1 M 

NPI=NPI *3 

71 ♦ 

8C7 

CONT I NU £ 


7 2 * 


If INPX.EQ.GI GO 10 8 1 C 


73* 


IF1NPI.EQ.MEP) GO 10 SZG 


14* 


M-T AM ( L > 


75* 


I a hi l no 


76* 


GO ! 0 805 


77* 

820 

CONI INUE 


79* 


TAM1L1-0 


79* 


DO 327 J- 2 • N 9 


80* 


IF 1 CPS IK*JI*EG«1> GO TO 828 


3 1 * 


GO TO 327 


82* 

B2B 

NN=NN* 1 


83* 


T A Mr 1 


84* 

62 7 

CONTINUE 


85* 

810 

CONT INUE 


86* 

C 



87* 

c 

LOCATE TERM. APPENDAGE 3ASE 90DIES A N3 

NO. OF T. 

8 0* 

c 



89* 


00 9 36 L ' 1 • N 0 


90* 

836 

NOME IL T-0 


91 * 


00 935 L^liNH 


92* 


DO 6 34 K- 1 iNH 


93* 


I c < I A M F It > . N £ • K 1 GO TO 934 


94* 


N -B t K > *1 


95 * 


QO 9 3 3 J- 2 a N 9 


96 * 


IF (EPS IK p J 1 .EG.G1 GO TO 8 33 


97* 


N U M r INJ-NUMF INI * 1 


98* 

833 

CONTINUE 


9 9* 

834 

CONT INUE 


100* 

035 

CONTINUE 


101* 

C 



10 2 * 

C 

FIND BASE 800V WITH THE HOST TERMINAL 

APPE NDAGE 

103* 

G 



10 4* 


N M X - 0 


10 5* 


00 837 L-JtNa 


106* 


IF INUMF 1 L > * G£ . NHX 1 GO TO 038 


1 G 7 * 


GO TO 937 


30 8* 

83 8 

1 AB-L-I 


109* 


NMXZNUMF IL 1 


no* 

837 

CONTINUE 


hi * 

C 



312* 

c 

IDENTITY MEMBERS OF THE TERMINAL APPENDAGE SET 

113* 

c 



114* 


00 698 L = 1 • N H 


115* 

898 

t itijzc 


116* 


IF < TAB.EG.NH> GO TO 84G 


117* 


DO 9 39 J = 1 iNH 


ns* 


K=T AMF IJ> 


119* 


XHHIKI .EQ.TABI GO TO 829 


120* 


GO TO 839 


1 21* 

829 

CON! INUE 


122* 


DO 888 L=2tNB 


123* 


IMEPSU.L) iEQ.l > jxil-ii=i 


124* 

8B8 

CONTINUE 


125* 

839 

CONT INUE 


126* 

840 

CONTINUE 


127* 

C 



12 6* 

c 

INITIAL CALCULATION OF BARKCENTER VECTORS H.R.l 

129* 

c 

AND HINGE POINTS 


1 30 * 

c 



131* 


ixxm=HBin 


132* 


mu 1 -MB 1 2 T 


133* 


IZZC 1 1 -HB 1 1 1 


1 34 * 


IXV U » = MBI N I 


1 35* 


IXZUKHBISI 



book c.g.s 


EACH 
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13 6* 

1 37* 
136* 

1 39* 
14D* 

3 43 * 
142* 
143* 

1 4 4 * 
145* 
146* 

14 7* 
140* 
149* 

1 50 * 

151 • 
152* 
153* 

1 54 * 
155* 
156* 
157* 
158* 

2 5 9* 
160* 
161 • 
162* 
163* 
164* 
165* 
36G* 
167* 
168* 

1 S3* 

1 70 * 

1 71 * 
172* 
173* 
1J4* 

1 75* 
176* 
177* 
178* 
179* 
ISO* 
1 81 * 
18 2* 

1 83* 
184* 

2 85* 
186* 
1 07* 
18 8* 
189* 
190* 
X 91* 
192* 
1 93 + 
194* 
1 95* 
1 56 * 
1 97* 

19 8* 

1 99* 
ZOO* 

2 G 1 + 

20 2 * 
203* 
20 4 * 
205* 
206* 


I Y2 O >=MBI6) 

BM AS S I 1 1 = M3 I 71 
TK = 0MASSU 1 
DO 35 J=2*Na 
IXXtJltMA f J~ 1 * 1 ) 

XJf »f J > = MA ij-h2> 
112 I J 1 = M A I J“1 t 31 
IXYW)=MA4J-lf4J 
1XZ IJISMA1 J-l *5) 
IYZ4 J)=MA U-l*5 I 
BMASS t J-2 »7 I 

35 !M=TM*8MA SS4JI 
00 149 1=1 »NB 
13=1-3 

DO 149 J=1 iNB 
J1=J-1 


IF 4 I, EG. J) 

GO 

TO 

263 

IFIXftGT.Jl 

GO 

TO 

70 

IFCIiEGftlJ 

GO 

TO 

80 


1= I EPS < II «J 7 -EQ . 1 ) GO TO 4 DC 
70 LX(I'J> = PM11 *11* Xt 

1_Y1 I*J l-PAI Ilf II » 2 I 
UUtJ!:PAI II .Hi 31 
GO TO 143 
400 CONTINUE 

00 £00 K = I * J 1 

I F I EPS I K • J I * E 0 , 1 ) GO TO 500 
SCO CONTINUE 
GO TO 149 

500 LXl 1 1 J I = P A I 12 #Ki 11 
LYtl. JI=PA I II .m2 1 
L2< I»J)=P4! Ii#Kt J) 

GO TO 149 

00 DO 9 0 L = 1 . J 1 

I F I EPS (LiJlftEQil) GO TO 101 
9 C CONTINUE 

GO TO 149 

101 LXC I • J 1 =P fl 1 L *11 
L Y 4 I . J I =PB l L .2 1 
LZ4 I# J1=PQ4L t 3) 

GO TO 149 
163 LX<rfJ7=0. 

L V 4 1 . J 1 = 0 • 

L 2 4 1 i J t =0 • 

149 CONTINUE 

DO 13 N = 1 » N 9 
DO 13 J=IfN0 
QX(NtJ)=lX4N i J I 
D Y 4 N • J I = L Y { N i J 1 
D24Nf J)=LZ4 Nj J) 

DO 13 K”3 . NB 

DX4NtJJ=OX<N#J)-<eMASS<KWTMI*LX4NiKt 
DY4N.J)=DY4 NiJI- 4BMASSIK»/TM1*LY4N.K1 
U D 2 I N#U1=D21N • J)** 4 0 HA S S t K I M M I *L24NtK I 

C 

c calculation of augmented inertia dyaqics for each body 
c 

00 31 N = 1 pN 9 
PHiNf 1 .1 »= IXXI N1 
PHINfl f 2 ) = - I X V 4 N 4 
PH(N.l f 3 1 = - IX 7 INI 
P-44 Nt 2f2l=IY Y4N) 

PHtN>2i3)=-IY2( N I 
PK1Nf3* 3UI2 2 4N I 
DO 30 J= J f NB 

P9|N«I fll=PH4Ni 1 til *6 MAS St Jl* TOY IN# J)**2*OZ4 Np Jl **21 
PHlNvl i2T = PHIN«lf?l-&MASS 4 J ) * D X 4 N f J 1 * 0 Y 4 N i J I 
P4IN# 3# 31 =PH IN 1 1 #3 J-BHA SST JJ* DX I N p Jl *DZ 4 Nf Jl 
PHIN*2 i2l = PH4Nf2i 2 I *BMASS 1 J>* IDX 4Nf Jl **?*DZIN* J l* + 2 2 
PM NfZt 31=PHTNi 2*3|-BHASS< Jl* DY 4 N t Jl * 3 Z 1 N • J4 
30 PH4Nf3*3)=PH4N*3*3T*BHASSIJI+tDXINfJT*+2*DY4NfJl**21 
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207* 


PHINt 2. 1 1 =PH tN . 1 »Z 1 

2D 8 * 


P H < N » 3 il l=PHIN»li3) 

2D9* 

31 

PH(N»3t21=PHIN»2 .31 

210* 


DO 57 J R-l *N6 

211* 


DO 571 J=1.3 

212* 


DO S 7 1 K = J (3 

213* 

5 7 J 

PStR.R.J*K»=PHIR . J.K| 

2m* 


DO 763 J=1.NH 

215* 


G1 S ( J 1 = 6 1 J • 1 1 **2 

21 S * 


G2$t J1=G» Ji2)«*2 

217* 


G35IJI=GIJ.3 1 « « 2 

218* 


G 1Z < J) =G 1 J . 1 I *G| J »2 1 

219* 


G13t JI = GI J.l ) *31 Jt3» 

220* 

763 

G23 J J1 = G 1 J*2 1 *61 J • 3 1 

221 * 


DO 3700 R=2.N9 

222* 


DO 37CG S=2.NB 

2 2 3* 


IHR.EQ.SJ SO 10 3700 

224* 


IP 1 III 1R-1 ) ,N£ .1 1 .OR . 1 71 IS -1 » ,NC • 1 M GO 7 0 3700 

225* 


0X2: OX 1 R. SI * □ X 1 S ,R j«TM 

228* 


DY2=D» IR.S l»DY(S.RI*tM 

227* 


OZ2=D2IRtSI*OZTS«R}*TH 

22 8* 


PSIRrS f } >1 )=-0YZ-0Z2 

229* 


PStR,S.li2)=DXIS.R)*0YlR.SI»tH 

2 30 * 


PSIRiS . I »3 ) = 0 X t S . R 1 *0Z ( R » S 1 < 1H 

2 31 * 


P5IR»S«2«]l = DYIS.RI*0Xlff»Sl»lM 

232* 


PST R.S .2 .2 JI-DX2-0Z2 

2 33* 


PSIR.SiZ.3l=DYfS.R)*02(R«SI*TH 

2i4* 


PStR.S.3«ll=DZIS.R)»DXIR.S)«lM 

2 35 * 


PS(R.S»3«2»=DZ(S.RI*DYIR.SI«TM 

2 3 6* 


PSIR.S .3.3 IS-0XZ-0Y2 

237* 

3 7 D 0 

CONI INUE 

238* 


11=0 

2 3 9* 


DO HC1 1=1, NH 

240* 


IF 1 11 ( I 1 ,NE .1 1 GO 10 1 *«0 1 

2*1* 


IT=IM1 

242* 


IFIPim.EQ.il 17 = 11-1 

24 3* 


Kf =0 

244* 


DO mtJC K=1.NH 

245* 


If(UIXI.NE.I) GO 10 1400 

246* 


KT =KT*1 

2 4 7* 


IF (PIIKl.EQ.lt K I = K I - 1 

24 8 * 


IFtl.Gl.Kt GO 10 1400 

2 4 9* 


DO 1410 0=1.3 

250* 

3410 

cut J1=0. 

251 * 


DO 1800 R=Z.NB 

252* 


DO leOO S=2 «NB 

253* 


I : U EPS IK »S 1 . EG . Cl .OR . t£ PS 1 1. R1 .EG. 0 1 1 GO TO 1800 

254* 


DO 1802 J= 1 » 3 

255* 


CE( J 1 =0 . 

256* 


DO 1 902 E = 1 . 3 

257* 

18C2 

C£<J|:CEIJitPStR.S»J«LJ*GIK.Ll 

258* 


DO 1*1311 J= 1 ■ 3 

259* 

1430 

GUI J1=CWIJ1 ♦CEJJ 1 

260* 

3 8CP 

CONTINUE 

2 6 1 * 


AS Bt I.K ) = G<I,1)*CUI1HGTI«2I*CUIZHGII.3I»CU43I 

262* 


IFIIPIIIt.E9.31.OR.IPIIK1.EO.lt) GO TO 1400 

2 63* 


AH IT.K7 1 = ASBII.K! 

264* 


A1<Kl.III=AItII.KTt 

265* 

1400 

CONI INUE 

266* 

1401 

CONI INUE 

267* 


NH 1 = K 1 

26 8 * 

C 


2 6 9* 

C 

INVERT THE TERilNAL APPENDAGE MATRIX OF C0NSIAN1S 

2 7D * 

C_ 


271 • 


I'INMI.EG.OI GO TO 1422 

272* 


CAUL A IN VO (At .G.NMT *$1422. OR K) 

2 73* 

1422 

CONTINUE 

274 * 


RETURN 

2 75* 


ENTRY HRA1ElNH.1H.TB.TA.F3.FA.3H.GH0.GHDD.U0.ECt 

2 76* 


DIMENSION 1BT 3 It TA tN«i3t .FB (3 1. FA 1 NH . 3 |.GH It I .GMDTI I.GMOO )1 I. 

2 7 7* 


AUDI 31 .UXQIU) .UYO tUl.UZOTUJ . EIS3.lt .1HINHI 
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278* 


DOUBLE PRE CISIGN EC IV ) 

2 7 3* 
2804 

C 

c 

BOOY-TO-BODY COORDINATE TRANSFORM* t IG N MATRICES 

281 ♦ 

c 


20 2 4 


DO 335 J=1 *NH 

2 03* 


K=Hl J 1 



IFIL1IJI.E8.1> GO TO 5021 

2 054 


S3M=SlNtGMtJ 1 1 

2 8 E * 


CGM-SQR! 1 1 • -$GM«» 2 1 - 

2 874 


IFTA3SIGMIJll.LT .1 1.5707963271) GO TO 5020 

2 8 8 * 


IF < ABS IGMI J » 1 ,Gt. 14 .? 1 2380381 T 1 GO 10 5019 

2 83* 


CGM- - CGM 

230 4 


GO TO 5020 

2 31* 

5019 

l~\ ASSIGMIJll.GI .17.853981) COM: COS ( GMT J II 

2 9 2 • 

5020 

CONTINUE 

2 93# 


C S Ml = 2 . - C G.M 

294* 


G12CG=G12I Ji*CGMl 

2 95 ♦ 


G1 3CG=G1 3 1 J ) *CGM 1 

236* 


G 23 C 3- G23 1 J 1 * C GM1 

2 37 4 


GISG'GIJ. H * SGM 

2904 


G2SG-GIJ.2 >*SGM 

2994 


6JSG:GIJf3MSSM 

3004 


8 A 1 1 .1 l = CGMtGlS 1 J 1 • CGM1 

301* 


BAT l .2 1 = G3SG *G12CG 

30 2* 


BA C 1 .3 1S-G2SG* G13CG 

30 3 * 


9A (2 .1 1 = - G1SG*G1 2CG 

30 4* 


B A ( 2 »2 ) - CG M « G2 $ 1 J 1 * CGM 1 

3 05 • 


8AT2»3J-G1SG*G23CG 

30 6 * 


B A 1 3 • 1 1 =G2 S G« G 1 3C G 

3 G 7 * 


BA«3.2»=-G1SG*G23CS 

30 B * 


BA13.3 1 = CGM <G3S T J 1 * CGMl 

3 0 9* 


GO TO 5C.2 2 

310* 

50 21 

CONTINUE 

311* 


BAT 1 . 1 1=1 . 

312* 


BAT1.2t=GIJr3l«GHTjl 

313* 


BAT 1 f 31 =- Gl J *21 * GM 1 Jl 

3144 


BAI2.I 1 - * B A < 1 *2) 

315* 


BATE .21 = 1 . 

3164 


BAT2.3l=GtJ.ll«GMIJl 

317* 


BAT 3.1»=- 8A 1 I .31 

31 8* 


9 A 1 3 »2 >=-BAI2. 31 

319* 


3A<3>3I-1. 

320* 

50 2? 

CONTINUE 

321 * 


1FIX.EQ.G) GO TO 3350 

32 2 * 


DO 33M L = 1 .3 

32 3* 


00 33<T M= 1 » 3 

324* 


T I) .J+l »L»H)=0 . 

325* 


00 3 3V 1=1.3 

326* 

334 

T (1 » J+ 1 » L »M 1 = t 1 1 » J ♦ 1 .L.MI+BAIL • I >* T TI *K«I . I » M 1 

327 * 


GO TO 335 

328* 

3350 

CONTINUE 

3 2 3* 


00 3351 L = 1 * 3 

330 • 


DO 3353 M=l»3 

331* 

3 35 1 

T( 1 • J. 1 .1 .» ) :BA 1 L. Ml 

332* 

335 

CONTINUE 

3 3 3* 

C 


334* 

C 

COORD. TRANSFORMATION OF G VECTORS ItO REF. BODY FRAHEI 

3 35* 

C 


336 * 


DO 3G2 ISJtNH 

3 37* 


DO 362 J = 1 • 3 

339* 


GOTI.JI-O. 

339* 


OD 362 K = l»3 

340 • 


GOT I r J 1 - GO 1 I . J 1 ♦ T ( 1 «1«).K.J)*G 1 1 » K l 

341« 

362 

CONI INUE 

342* 

C 


3 4 3* 

, C 

ANG. VELOCITY COMPONENTS OF EACH BODY TIN REF. BODY FRAME! 

344* 

c 


345* 


DO 361 3=1. N3 

3464 


W XO 1 J 1 = WO 1 1) 

34 7 * 


WYOT JUM0T2 t 

340* 


U20TJ)=W0t 3) 

349* 


DO 36 K = 1 « M H 
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3 bO 
3 51- 
352 
3 5 3' 
354 
3 5 5 i 
356 
3 5) 
350 
3 5 9< 

360 

361 » 

362 
363- 
364. 
3 65 < 
366 1 
367 
360 
3 69 
370 
3 7J 
372 
3 73 
3?4 
3 75 
376 
3 77 
3 78 
3 7 9 
380 
341 

382 

383 

384 
3 85 
366 
337 
308 
383 
390 
3 91 
392 
3 93 
394 
3 95 
336 
397 
390' 

3 99' 
400 ' 

4 D 1 < 

402- 

403- 
40 4 1 
405- 
406' 
4 0 7> 

40 8" 
4 G 9< 
410 
4 11' 
412 
413 - 
414 
4 15- 
416 - 

41 7< 
41 0 ' 
4 1 9< 
420' 


IF (EPS (K* J UEQ.OI GO 7 0 36 
WXOIJ)-WXOiJMG*Om*GC>tKfll 
W VO I Jl =U VO I J> * 6MD IK > »GOIK tZ I 
M2O«JI = M2OIJMGMi)IKMG0lKi3l 
36 CONTINUE 
361 CO NT INUE 
C 

C TRANSFORM AUGMENTED 90 D V INERTIA OVAOICS TO REF , 9007 FRAME 

C 

DO 363 1-2 • N 8 
DO 364 J- 1 * 3 
DO 36 4 Kr U 3 
A 9 ( JtK 1=0, 

00 364 L -1*3 

ABt JtKUAB l JiK UPHt I, JtL) *T H # ItLt* I 

364 CONTINUE 

DO 365 J~ 1 1 3 
00 365 K - 1 • 3 
ps i r • i » j»k i=o . 

00 365 L - 1 p 3 

P5(ltl»J«KUPSII*X»J»KIMU«I»L»J)«ABIL*Kl 

365 CONTINUE 
363 CONTINUE 

00 4 7 1 J = 1 • 3 

DD 471 K=1 t 3 1 

‘471 PSI I 1 1 # J «K I =PHU t J#K I 
FEX0I1 >-FBm 
FEVGm=F6l2l 
FEZCil I = F Q I 31 
T X Of J J = T a ( 1 1 

no U UTB12I 

T20I 1 1-TBtJ) 

00 17 I = 2 r N0 
K-I-l 

F_EXO(I):TlliI»]»l MFMK»lhTllirt2»n*FA(K»2|4lU»Ii3»l I A I k # 31 
F.yo 1IJZT (1* I«l«2J*FA(Xt]) ♦ T < 1 * I i 2i 2 }’fA(K«2J»Tll»I« 3* 2 I *FA ( Kt 3 ( 
FE70m:Tinii3»3nFA!KtnM (1 »I*Z»3 MFMKp 2)*H1 il >3p3l*FMKt3) 
TXO( I J =J(1 p lilt 1 J * T A ( K i 1 ) *T I 1 • I # 2 1 I )*IAIK«2MTtl«Ii 3,1 I * T A | K § 3t 
TYOdlz T I IpIpI *2 ><TA t K * 1 ) *T (1 »Ii2«2>*IAtKr2 I ♦ T (1»I t 3 • 2 } * T A t K t 3 ) 
1Z0T I)z 1 Hi lilt 3)»lAlKtl) t T llalt2«3)MAlX*2MTI l«I«3»3J*TAlKt 3 J 
DO 17 U= 1 t N 8 

DXO( ItJ I = T I 1 « I« lflJtDXIltJ) ♦ T U<Ii2« 1 J * Q Y ( It J J * I UtltT.l MDZ I It J J 
DTO ( I > J 1 Z 1 42»I»l»2)*OX (I»J>«TlltIi2f?|*0YtItJ]«Tll • 1 *3 » 2 I * C2 t It J ) 
DZO( ItJ I z T < 1 1 It 1 « 3)« OX I It Jl «1 <1 t I«2» 3 ) *DY i It U I *1 U 1 1 » 3 • 3 I ♦ OZ t It J I 
17 CONTINUE 

DO 3 6 7 1= 1 « N 9 

oxo u iitzoxn 1 1 1 

0 Y 01 liI):OY(M) 

36 7 OZOU tIUDZU til 
C 

c additional augmented inertia dyadics iin ref, body framej 

c 

DO 37 1 = 1 1 MB 
DO 3 7 J = 1 t N 3 
IF I X.EQ* JJ GO TO 37 

PSlXaJiltllz-T** (OYO 1I» U) ♦ D Y 0 I Jp I) •023IItJJ*0Z0IJtIII 
PSI IiJtl t2 > = TM*DXO(J. I)*DYOIItJI 
PS( ItJil » 3 I = TM * 0 XO IJa 11*0201 I # J) 

PSIIt J»2#l )z1M*DY0» Jill *0X0(1# J) 

PStXaJtZfZJZ-TM* (0X9 (I«JI*OXOIJil) tDZOI I«Jl*L)Z01JtIl I 
PSf 1 1 Ji2#3 lz|H*DYO t Jt I)*020 lit J) 

PSI I»Jf3il) = lM*D20 IJiIMDXOJIiJ) 

PSCXiJ>?»2»=YK»OZOIJ«XI*OVOll«J) 

PS( I#Jt3t3t=-TM* !0X9 <ItJJ*DXOIJ«X) total ItJI*OYOIJ*It I 
3? CONTINUE 

C 

C VECTOR CROSS PRODUCTS DESCRIBING SYSTEM ROTATIONAL COUPLING. 

C (QUADRATIC TERMS INVOLVING THE CONNECTING BODY ANGULAR 

C VELOCITIES AND THE MUTUAL BARYCENTER- HINGE VECTORS) 

C 

DO 230 N=1 t N0 
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<121 
422 
<12 3 
424< 
425< 
42 6' 
4 2 7 4 
426' 
42 9< 
430' 
431i 
432 ' 
4 33- 
434' 
4 35i 
436' 
4 3 7 i 
4 3 B 
4 3 9' 
440- 

441 < 

442 

443 

444 
445' 
446 
44 7 
44 8 
44 9' 

450 

451 
462 

453 

454 

455 

456 

457 

458 
4 59- 
4 60 ■ 
4 61' 
4 6 2 1 
4 6 3« 
464 
465- 
466 

46 7 

468 

469 

470 
4 71 
472 
4 73 
474 
4 75 

476 

477 

478 

47 9 
460 
4 01' 
482 
4 83 
484 
4 05' 
486 
4 87 
486 
4 99 

490 

491 


CP X " 0 • 

CPV-0. 

CP2=G. 

00 2301 l=l»N6 
I F ( N • EQ . L ) GO TO 23C1 
WOX=WtCIL)*OZOiLfN) -WZ Of L 1 *D YO IL i N ) 

KDY=MZQ<L 1*0X0 iL iN ) - MX 0 < L I • 0 I OIL t N) 

W0Z=WX0IL)*DY0IL»N1 ~ k y OIL >*0X0 i L « N 7 
UUDX = fH» JHV0 (LI • U92-ii201 Ll* U9 V) * F EX 0 I L I 
KWDYUM* U20IL )*W OX -WXOU 1 *MDZ HFE YOU. > 

UUDZMH* UXOIL I » iJOV-UVOU)«tiDXJ «F EZO IL 1 
DWVJDX = DYCINtL ) * IrfWDZ - GZOTNtl 1 * H WO Y 
OWWDY=DZOlN.L ) HOX-OXOI N» U * WwDZ 
DUHDZ = DXOINiL>*WKDY-OYOINfLI*WWDX 
CP X = CPX ♦DHWOX 
CP 7 = CPY «DWHDY 
CP2=CP2*OWrtOZ 
2301 CONTINUE 

DFX=DYG1NtN 1 *EEZQ<N 1-020 I N » N1 *FEYOI NT 
OFY = DZOlNtM*F£XO IN 1-DXOINtNMFEZO IN) 
Q c ZrOXOINiNMFEYO<NJ-DYOIN*N)*FEXOINJ 

HX=PS IN.N.I tl 1 *WXOINMPS INtNtl .2 l*WYO I Nl *PS INtN.l .3 1*KZ01N 1 
HYr?SlNiN#2»l)*HX0IN) ♦ ?SINtN#2#2)*WY0INI*PS (N.N*2*3)*KZ0IN1 
HZ-PS I N . N * 3 . 1 I ♦WXO(N) ♦ P 5 I N » N » 3 « 2 I » U V 0 4 N 1 ♦ P $ I N t N 1 3 » 3 I * W 2 G I N I 
K = 3 * I N- 1 > 

E<KM#l)=HY*kj20 IN > - HZ •UYOlN»*TXOIN)*CPX*DFX 
EIK»2fllSH2«UXOINI~HX«HZOINMIVO<NltC>V«OFV 
EIK«3«1 1=HX«UV0*N J-HV*UXOlN>*f 20 INI«CPZ«DFZ 
230 CONTINUE 
C 

C ADO MATRIX ELEMENT COMMUTATION I 3 X J » 

C 

00 3C01 1:1.3 
DO 3001 J- 1 • 3 
3 GO 1 AOCI I.J 1=0. 

00 3 1=1 tNB 
DO 3 J=3 rND 

m-ii«n = A0Dii«]i4psti'j»] ii) 

A0C1 I tZUAOG Hi 2 I «P S t It Jt 1 t 21 
AGQIlt31=A00l 3 • 3 1 ♦ P S I I • J 1 1 • 3 ) 

ADOI 2.21 = AGGI2i2 MPSlIi Jt 2«21 
*0QI2>31=«DDI?«3l «PSU»Ji?t3> 

AGO! 3.3 l = ACO <3t3 HPS1 It Jt 3i 3J 
3 CONTINUE 

AQOr 2.1 I = AG 0 111 2 ) 

A0CH3 1 1 1 = A D0( 1 # 31 
AOOI 3 . 2 ) = ADO 12* 3 J 
C 

C AOK VECTOR ELEMENT COMMUTATION 13X11 

C 

00 7 K-l.NH 
A yiK.l 1=0. 

AVtK#2l=G* 

A V I K . 3 1 = 0* 

00 7 1=3 * N 3 

DO 7 J = 1 tNB 

IF1EPSIK»J)*EG«GJ GO TO 7 
00 1U Mr 1 • 3 
00 10 U‘l .3 

10 AVIK'M>=AV1KvHI«PSIX»JtMiL>«604K»Lt 

7 CONTINUE 

C 

C AKM SCALAR ELEMENT COMPUTATION 

C 

00 14 X = 1 iNH 
00 14 M=ltNH 
1 C (K.GT*M> GO TO 14 

IF I f 1 1 IK 1 . EG. U -AND . ( T I ( H I -EG. 11 1 GO TO 1339 
AISI 1 1=0. 

A I S 1 2 » =0 » 

AISI 31=0* 
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**92* 

4 93* 
*494* 

4 95* 
496* 

4 97* 
498* 

4 93* 
SDG* 

5 D 1 ♦ 
50 2 * 
503* 
50 4 * 
505* 
506* 

50 7 * 
50 8* 
503* 
510* 
511 ♦ 
512* 
513* 
514* 
515* 
£16* 
517* 

51 8* 
519* 
520* 
521* 
522 * 
5 2 3* 

52 4 * 
525* 
526* 
SZI* 
5Z8* 

52 9* 

530 • 

531 • 
532* 
533* 
5 34* 
535* 
5 36* 
5 3 7 * 

53 8* 
5 3 9* 
540* 
5 4 1 * 
542* 
543* 
544* 
545* 

54 6* 
5 4 7* 
54 8 * 

54 9* 
550* 
551* 
552* 
553* 
554* 
555* 
558* 
5 5 7 * 
558* 

55 9* 
560* 
561 ♦ 
562* 
563* 


DO 15 J= 1 > Nfi 
00 15 1=1 • N 9 

Iff (£P5 < K . J ) . £ Q «Cl I « OR « (EPS I H 1 1 1 •£ Q *0 I ) 0 0 10 15 
DO IB N ” 1 i 3 
00 16 L = 1 * 3 

10 AIStNI = USINl«PSlJ»I'NfL)*GOItt«LJ 

IE CONTINUE 

A$t*tMT = GOUin*AISUI*GaTK.2l*AISt2MGOTK t 31*AIS<3l 
GO TO 14 

1399 ASI < #M1=AS3 IK • *) 

14 CONTINUE 

C 

C COMPUTE CORRECTION ELEMENTS FOR IE J VECTOR 

C 

DO 40 I - 1 ► N8 
£AI 1 7 = 0* 

E M 2 I = C » 

E A < 3 J =X • 

00 41 J - 1 • N B 
00 4 1 1 M= 1 « 3 
411 CWIK)=L. 

00 42 K = 1 &N H 

IF (EPS IK » J I « E Q ,01 GO TO 42 
K9 = KKMI 

C W ( 1 J - CW | 1 l i GM 0 (K I * |M¥0lKB)*G0tK»3T-UZCHK8)*GGIKf2 I) 
CWl2)=CUI2J«GMDIK|*INZO<K9)*G04KfU-HX0IKBI*G01Kf3l! 
CUI3UCU 13 MflMOU)* |UXolK8l*G0 t K • 2 > -U * 0 I K B I • GO I K * 1 II 
42 CONTINUE 

DO 45 Kr J, 3 
DO 45 L= 1 <3 

45 EMM> = EMHI«PSIX»JrK»Lf*CU!L> 

41 CONTINUE 

Ki = 3*n-n 

E< Kl* I • 1 l=EIKl «1 *1 l-EA 111 
E ( K 1 *2 » 1 I = E I K 1 * 2 * 11-EA 121 
£ I K1 + 3* 1 1 =E<K1 *3 *1 l-E A* 31 
4 0 CONTINUE 

DO 55 M I - 1 i 3 
55 ECIMI I =E I M I r 1 I 

DO 52 J=2»NB 
DO 52 H= 1 1 3 
Kl = 3* I J- J J*N 

52 ECIMUECIM ME (K)tl » 

X = 0 

DO ED Kil*NH 
IF |PI1K!«NE*D1 GO TO 60 
1=1*1 

ECI I* 31 = 0. 

00 601 M- 1 *3 
601 CSIN)=Q. 

DO 61 J=MNB 

IF T EPS tK » J 3 .EQ.OI GO TO 61 
00 85 M=1.3 
J] =3*IJ-1 J *N 

65 CECMI-CE iH) ♦£ 4J1* 1 I 

8] CONTINUE 

DO 86 L= 1 » 3 

66 EC1 I* 3) = EC1 1*31* GO IKiLMCEUI 
ECl 1*3 l = ECII*3MTHtK I 

60 CONTINUE 

DO 610 1=1.3 

00 610 J - 1 t N H 

IF t PI t J I •EQ.OI GO TO 610 
EC4IUECI IMAVI JtlMGMDDI Ji 
610 CONTINUE 

K = 0 

1 W - 3 

00 512 I=lfNH 
IF(PIUKNF.O) GO 10 612 
Ht = K* 1 
IV=IV*1 

DO 511 Jnl.NH 
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564 * 
565 * 
566 * 
561 * 
ESS* 
563 * 
57 D* 
5 ) 1 * 
572 * 

5 7 3 * 
574 * 

5 7 5 * 
576 * 
5 7 7 * 
578 * 
5 7 9 * 

5 BO * 
5 81 * 
56 2 * 
583 * 
58 4 * 
585 * 
5 B€* 
5 8 7 * 
568 * 
589 * 
530 • 
591 * 
532 * 
5 93 * 
534 • 
5 95 * 
536 * 
5 97 * 
596 * 
5 99 * 
600 * 
sai* 


IF (PI I Jl.EQ.G) CO 10 611 
I c ( I « G J . J ) A S 1 I * J) = A S t J « 1 1 
ECIK «31 = ECIKOI-AS 11 ? JMGMOO I J) 

611 CONTINUE 

612 CONTINUE 
C 

C LOAD SYSTEM MATRIX ( A V WITH AGOpAOK*AKM ELEMENTS 

C 

00 23 1 = 1*3 
00 23 J = 1 *3 
2 3 AlI.JUftCGlIiJJ 

aa 24 i=i *3 
K =0 

00 24 J = 1 iN H 

IF IPI I JI.NE'G I GO 10 2 4 

K = K* 1 

A (K *3 * I > = A V I J* IT 
A ( I*K* 3 l = AV I J# I) 

24 CONTINUE 
K = G 

00 250 2 = 1 *NH 
I c (Rim.NE,Cl GO TO 25 D 
K = K + 1 

1 = 0 

DO 25 J = 1 » N H 

IF< P I( J I ,N£*G 1 GO TO 25 

L =L + 1 

Ir { < «GT .L ) GO TO 26 
A IK *3 * L * 3 1 =ASII*JI 
GO TO 2 S 

26 A ( K * 3 p L * 3 I =A(L* 3 iK *31 

25 CONTINUE 
250 CONTINUE 
C 

C SOLVE SYSTEM MATRIX FOR REFERENCE BOOY ANG. ACCELERATION A NO HINGE 

C (RELATIVE) ROTATIONAL ACCELERATIONS 

C 

C EXCHANGE ROWS IN A-MAHIX A NO RT-HAND SIDE VECTOR 


6 G 2 * 
6 C 3 * 
604 * 
6 C 5 • 
606 * 
60 7 * 
£0 8 * 
609 * 
610 * 
611 * 
612 * 
613 * 
614 * 
615 * 
616 * 
617 * 
618 * 
6 19 * 
620 * 
6 2 1 * 
622 * 
6 23 * 
624 * 
625 * 
626 * 
6 27 * 
628 * 
629 * 
630 * 
631 * 
632 * 
S 33 * 
634 * 
635 * 


C 

00 619 K = 1 *3 
EEIKUECIK 1 
00 619 I=1?IV 

619 AE I K» 1 1 = A I K*I I 
L = C 

DO 620 K= 1 * NH 

IF I T IK ) -EG. 1 ) GO TO 620 

L = L *3 

£EIL* 3 )=ECIK* 3 ) 

DO 621 1 = 1 *IV 
621 4 E!L* 3 *I)=AIK* 3 fI) 

620 CONTINUE 

QO 6 3 0 K - 1 iN H 

IFUI I K I • £ Q »Q 1 GO 1 0 6 30 

L = L* 1 

££U *3 )=EC IK *3 I 
DO 631 I- 1 * I V 
631 A£tL* 3 *I)=AIK* 3*1 I 
630 CONTINUE 
C 

C EXCHANGE COiUMNS IN A-HATRIX 

C 

DO 539 K = 1 * 3 
00 639 1 = 1 t IV 
633 A ( 1 . K ) = A E 1 1 • K ) 

1=0 

00 G 40 K= 1 *N H 
IF MlttO'EO'l > GO 10 640 
L = L* I 

00 641 1 = 1 .IV 
641 At X«L* 3 ) = A£( I«X* 3 I 
640 CONTINUE 

DO 650 K = 1 »N H 
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636* 


imitKl.EQ.O) GO TO 6 50 


S3?* 


L = L* 1 


63 0 * 


DO 651 1 = 1 > IV 


8 3 9* 

651 

A< I » L ♦ 3 > = A E 1 Ii K * 3? 


640* 

65C 

CONTINUE 


□ 41* 

C 



64 2 * 

C 

SOLVE FOR HINGEtANGLE ACCELERATIONS 

1 NON-TERM, APPEN0A6EI 

S 4 3 * 

c 



64 4 * 


NL=I V-NMI 


645* 


IMNMl.EQ.C) GO TO 5561 


646* 


00 652 1=1 * NL 


647* 


00 5 52 K = 1 # NM T 


6 4 8 * 


ALU *K ):0, 


54 9* 


00 6 52 L = 1 • NM T 


650 • 


LL = Nt + L 


6 51 • 

852 

AL< L«K l = AU I#K |* A< I, LL 1 • At <Li K1 


652* 


00 654 1 = 1 t NL 


6 5 3 * 


00 554 K = 1 t NL 


65 4 * 


A E f I * K 1=0- 


6 5 5 * 


00 553 L = 1 iNMT 


656* 


LL=NL*L 


651* 

653 

A I a K 1 = A E 1 I«K) + AL(IvLI*AILliKI 


65 0 * 

65 4 

A KliKl=AUtK) “ A E 1 ItK) 


653* 


00 556 1=1 i NL 


6 60 * 


ECtl):D. 


661 * 


00 5 55 L = 1 • NM T 


662 * 


ll = NL * L 


6 6 3* 

655 

EC{ I ) = E C 1 11 MU I iL1*E£IIU 


664* 

656 

EEIIUEEIII-ECIII 


665* 

6561 

CONI INUE 


668* 


CALL SOtUA tVtNL tEEiV M t*92tWPK 1 


6 6 7 * 

C 



66 8 * 

C 

SOLVE FOR HINGE-ANGLE ACCELERATIONS 

1 TERM. APPENDAGE I 

6 6 9* 

c 



G 7 D * 


IFINMT-EQ.G1 GO TO 6581 


6 71 • 


DO £57 1=1 i N X T 


672* 


1 1 = NL + I 


6 7 3 * 


E C ( I ! =£E 1 IX 1 


6 74 * 


DO G 5 7 L = 1 ► NL 


6 ? 5 * 

657 

E C 1 I J = E C t XI-AIII » L i * ££ 1 L 1 


676 * 


DO 656 1=1 iNHT 


6 7 7 * 


II=NL* I 


678* 


min:o. 


6 7 9* 


00 £58 L=UNM! 


660* 

658 

EE« U UEEI III MU ItL 1*EC (L 1 


581* 

6581 

CONTINUE 


68 2 * 

C 



6 83* 

C 

a- C0NS1RUC1 SO. U I ION VECIOR IN ORIGINAL ORDER 

684* 

C 



5 85 * 


ECU 1 = £EI 11 


666* 


E C ( 2 1 : E E t 2 J 


5 37 * 


EC< 3 I =££ t 31 


668* 


L=3 


6 89* 


00 559 K=1«NH 


690* 


IF 1 IT I IK) . EQ.1 1 .OR. IP IIKI .EQ.l 1 1 GO 

T 0 659 

531* 


LiL* 1 


692* 


ECIK+3 > = EE 11 1 


S 93 * 

659 

CON! INUE 


634* 


L -NL 


6 35* 


00 561 X - 1 » N H 


896* 


IFt ITIIM.NE.l 1 .OR. IPUKl .EG.l I > GO 

T 0 661 

6 97 * 


LiL* 1 


698* 


EC I K + 3 ):EE 1 L ) 


6 39* 

661 

CONTINUE 


700* 


00 662 K=J«NH 


701* 


IFIPllXI.NE.il SO TO 662 


702* 


EClKUUGMDOIKl 


7 C 3 * 

662 

CONTINUE 


10 4 * 

92 

CONTINUE 


70S* 


RETURN 


70 6 * 


END 
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